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ABSTRACT 


The  objective  of  this  thesis  is  to  investigate  greater  levels  of  autonomy  in  unmanned  vehicles. 
This  is  accomplished  by  reviewing  past  literature  about  the  developing  of  components  of  soft¬ 
ware  architecture  that  are  necessary  for  unmanned  systems  to  achieve  greater  autonomy. 

The  thesis  presents  implementation  studies  of  existing  sensor-based  robotic  navigation  and  map¬ 
ping  algorithms  in  both  software  and  hardware,  including  a  laser  range  finder,  on  a  quadrotor 
unmanned  aerial  vehicle  platform  for  real-time  obstacle  detection  and  avoidance. 

This  effort  is  intended  to  lay  the  groundwork  to  begin  critical  evaluation  of  the  strengths  and 
weaknesses  of  the  MOOS-IVP  autonomy  architecture  and  provide  insight  into  what  is  necessary 
to  achieve  greater  levels  of  intelligent  autonomy  in  current  and  future  unmanned  systems. 
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CHAPTER  1: 

Introduction 


1.1  Introduction 

In  this  thesis  work  we  are  interested  in  implementing  and  demonstrating  appropriate  path¬ 
planning  and  obstacle-avoidance  algorithms  using  a  unmanned  aerial  vehicle  (UAV)  platform 
in  order  to  accomplish  a  simple  military-type  scenario.  The  UAV  is  tasked  with  a  mission  to 
autonomously  transit  from  a  known  launch  site  to  a  given  target  location.  Onboard  sensors 
play  the  most  important  role  in  providing  mission-environment  information  to  avoid  obstacles 
encountered  during  the  mission. 

The  platform  of  interest  is  the  quadrotor  UAV,  which  is  a  rotorcraft  equipped  with  four  powered 
rotors  laid  up  symmetrically  around  its  center.  Quadrotors  are  considered  an  ideal  solution  for 
many  tactical  applications.  Because  of  the  low  platform  footprint  and  weight,  relatively  simple 
mechanical  design  and  control,  hover  capability  and  slow  speeds,  this  platform  became  very 
popular  to  fit  several  applications  needed  for  both  military  and  civilian  missions,  especially  for 
surveillance  and  indoor  navigation. 

UAVs  in  general  have  gained  popularity  for  surveillance  and  reconnaissance  missions  to  provide 
situational  awareness  to  ground-based  military  units  and  to  use  onboard  sensors  to  identify  or 
intercept  objects  which  represent  a  threat  to  the  safety  of  military  and  civilian  personnel.  The 
majority  of  UAVs  are  controlled  by  human  operators  from  ground  control  stations  (GCSs)  using 
radio  frequency  (RF).  Moreover,  UAVs  also  have  the  capability  to  provide  precise  and  real¬ 
time  information  to  an  on-scene  operator  with  high  resolution  images  from  cameras  and  other 
capable  sensors.  This  can  help  inform  a  commander  in  making  appropriate  safety  decisions  for 
his  unit. 

1.1.1  Motivation 

During  the  last  several  years,  the  Tunisian  Army  has  identified  border  security  as  a  high  prior¬ 
ity.  The  diverse  terrain  features  of  the  western  border  separating  Tunisia  and  Algeria,  for  ex¬ 
ample,  significantly  increase  the  difficulty  of  conducting  patrolling  missions  using  conventional 
approaches.  Nowadays,  borders  are  physically  protected  by  mounted  and  dismounted  border 
patrol  agents,  which  presents  both  a  strain  on  limited  resources  as  well  as  a  risk  to  personnel. 
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Despite  the  large  presence  of  national  agents,  we  still  fear  that  terrorists,  drug  smugglers,  and 
arms  dealers  can  exploit  existing  gaps  and  vulnerabilities  of  the  borders.  For  example,  in  2007, 
Tunisia  announced  that  it  had  killed  twelve  and  captured  fifteen  Islamic  extremists,  six  of  whom 
had  arrived  from  Algeria  [1], 

The  desire  to  use  UAVs  is  fueled  by  the  need  to  increase  situational  awareness  and  remote 
sensing  capabilities  for  such  border  protection  missions.  UAVs  have  attracted  major  defense 
and  security  research  interest  during  the  last  decade.  When  there  is  a  need  to  acquire  aerial 
surveillance  and  intelligence  in  challenging  environments,  such  as  dense  forests,  tunnels  or 
caves,  or  when  the  situation  makes  using  a  manned  aircraft  impractical,  UAVs  presently  are  the 
best  solution  for  keeping  human  life  and  expensive  aircraft  out  of  risk.  UAVs,  equipped  with 
a  camera  and  additional  sensors  for  navigation,  can  be  hastily  deployed  to  gather  information, 
especially  where  signals  from  satellites  are  either  unavailable  (e.g.,  area  is  not  covered  or  cost 
is  prohibitive)  or  unusable  due  to  low  signal  strength. 

In  this  thesis,  we  hope  to  develop  a  better  understanding  of  the  new  techniques  used  in  surveil¬ 
lance  and  reconnaissance.  UAVs  offer  major  advantages  over  traditional  methods,  not  only  in 
terms  of  performance,  but  also  increasingly  because  of  their  lower  relative  cost.  UAVs  have 
been  used  by  many  countries,  and  it  has  been  shown  that  they  can  provide  more  precise  infor¬ 
mation  than  other  stand-off  intelligence  sources.  In  war,  most  decisions  are  made  in  a  short 
frame  of  time.  The  accuracy  of  information  gathered  is  very  important  in  decision  making.  No 
one  wants  to  kill  innocent  people  because  of  lack  of  information,  while  on  the  other  hand,  no 
one  wants  to  take  the  risk  of  bypassing  an  opportunity  to  stop  an  enemy. 

1.1.2  Research  Questions 

As  can  be  seen  from  various  reported  experiments,  onboard  sensors  represent  the  quadrotor’s 
source  for  information  gathered  from  the  environment.  The  quadrotor  needs  this  information 
to  navigate  and  perform  its  mission.  The  first  question  one  must  ask  is:  what  are  the  minimum 
quality  requirements  that  one  must  impose  on  sensors  to  obtain  an  accurate  navigation  system? 

We  must  also  consider  cost.  If  UAVs  are  believed  to  be  useful  for  border  security,  what  are  the 
potential  cost  implications  of  UAVs  for  future  Ministry  of  Defense  budgets? 

The  last  question  concerns  the  feasibility  of  using  different  autonomy  software  architectures. 
This  research  evaluates  the  MOOS-IvP  architecture,  and  attempts  to  determine  if  this  archi¬ 
tecture  provides  the  best  solution  for  developing  and  implementing  the  algorithms  needed  for 
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navigation  and  control. 


Questions  1  and  2  will  be  answered  in  Chapter  3.  Question  3  is  addressed  in  the  experiments 
with  results  presented  in  Chapter  4,  and  the  answer  will  be  discussed  in  Chapter  5. 

1.2  Background 

1.2.1  Related  Work 

Signficant  work  has  been  done  developing  various  control  methodologies  for  the  quadrotor 
aerial  robotic  platform.  A  research  group  at  Stanford  University,  the  Stanford  Testbed  of  Au¬ 
tonomous  Rotorcraft  for  Multi  Agent  Control  (STARMAC),  is  using  a  quadrotor  to  study  new 
multi-agent  algorithms  to  avoid  collision  and  obstacles.  Others,  including  Hanford  et  al.  [2], 
have  tried  to  build  low-cost  experimental  testbeds  that  use  quadrotors  for  a  variety  of  research 
interests. 

Numerous  efforts  have  investigated  control  of  quadrotor  platforms,  ranging  from  simple  tra¬ 
jectories  to  dynamically  challenging  acrobatic  maneuvers,  using  various  control  methodolo¬ 
gies.  For  example,  Bouabdallah  et  al.  [3]  compared  the  stability  performance  of  quadrotors 
for  proportional-integral-derivative  (PID)  and  linear  quadratic  (LQ)  techniques.  Chen  et  al.  [4] 
derived  combined  model-based  predictive  controllers  for  quadrotors,  one  based  on  a  lineariza¬ 
tion  feedback  controller  concept  and  the  other  based  on  the  back-stepping  controller.  Johnson 
and  DeBitetto  [5]  present  a  guidance  system  which  generates  position,  heading,  and  velocity 
commands  based  on  the  current  state  of  the  UAV,  as  reported  by  the  navigation  system  and  a 
waypoint  list.  The  guidance  system  commands  a  straight-line  course  between  waypoints. 

Higher  level  autonomy  for  unmanned  systems  is  also  an  active  area  of  research  for  aerial  plat¬ 
forms,  although  much  of  the  previous  works  focus  on  helicopter-based  platforms.  Koo  and  his 
colleagues  [6, 7]  developed  a  hierarchical  hybrid  control  structure  to  govern  a  UAV  in  its  mis¬ 
sion  to  search  for,  investigate,  and  locate  objects  in  an  unknown  environment.  The  hierarchical 
architecture  consists  of  four  layers:  the  strategic,  tactical,  and  trajectory  planners,  as  well  as  the 
regulation  layer.  The  strategic  planner  coordinates  missions  with  cooperating  UAVs  and  creates 
a  sequence  of  waypoints  for  the  tactical  planner.  The  tactical  planner  can  land,  search  an  area, 
approach  a  waypoint,  avoid  collision,  and  inspect  objects  on  the  ground.  Additionally,  it  over¬ 
rules  the  strategic  planner  for  safety  reasons.  The  trajectory  planner  decomposes  commanded 
behavior  into  a  sequence  of  primitive  maneuvers,  guarantees  safe  and  smooth  transitions,  and 
assigns  an  appropriate  sequence  of  height  commands  to  the  regulation  layer  to  execute. 
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Kottmann  [8]  designed  a  trajectory  generator  which  converts  flight  maneuvers  on  an  abstract 
level  into  waypoints  that  must  be  contained  in  the  trajectory.  In  a  further  step  of  refinement, 
the  generator  splits  the  routes  between  waypoints  into  several  segments  of  constant  velocity 
or  constant  acceleration.  After  this,  the  ground  station  uploads  the  segments  to  the  helicopter 
onboard  system  for  execution. 

Lai  et  al.  [9]  present  a  hierarchical  flight  control  system  containing  three  tiers:  the  navigation, 
path,  and  stabilizing  layers.  The  navigation  manager  controls  the  mission  and  commands  a 
series  of  locations  to  the  path  controller.  When  the  path  controller  reaches  a  location,  it  switches 
its  destination  to  the  next  location.  The  stabilizing  controller  applies  the  nominal  values  from 
the  path  controller  for  both  attitude  and  height  to  the  helicopter. 

Kim  et  al.  [10, 1 1]  designed  a  hierarchical  flight  control  system  which  allows  a  group  of  UAVs 
and  Unmanned  Ground  Vehicles  (UGVs)  to  cooperate.  The  flight  management  system  em¬ 
ploys  three  strategy  planners  to  solve  specific  missions.  The  first  strategy  planner  implements 
the  simple  waypoint  navigation  of  a  predefined  course.  The  second  strategy  planner  operates 
a  pursuit-evasion  game  where  pursuers  capture  evaders  in  a  given  grid  field.  The  third  strat¬ 
egy  planner  executes  flight-speed  position  tracking,  where  an  UAV  pursues  a  moving  ground 
vehicle. 

Williams  [12]  developed  his  own  open  source  project  called  Vicacopter,  incorporating  two  ways 
of  autonomous  control:  waypoint  following  and  programmable  missions.  For  waypoint  follow¬ 
ing,  the  ground  station  uploads  a  sequence  of  waypoints  to  the  helicopter.  After  the  helicopter 
reaches  a  location,  it  heads  for  the  next  location.  For  programmable  missions,  the  ground  station 
uploads  a  series  of  control  commands  to  the  helicopter  for  execution. 

The  MOOS-IVP  portion  of  this  work  expands  previous  work  done  in  Unmanned  Underwater 
Vehicles  (UUVs).  In  their  seminal  paper  entitled  Nested  Autonomy  with  MOOS-IvP  for  Interac¬ 
tive  Ocean  Observatories,  Michael  Benjamin  and  his  co-authors  describe  the  relation  between 
the  MOOSDB  (Mission  Oriented  Operating  Suite-Database)  and  the  behavior-based  autonomy 
system  which  implemented  by  the  IVP  Helm  (Interval  Programming)  [13]. 


1.2.2  Different  Type  of  Quadrotors 

The  first  quadrotor  vehicle  was  built  in  1907  by  two  French  scientists  [14].  The  challenge  was 
to  make  the  machine  lift  itself  off  the  ground  using  its  own  power  and  pilot  itself. 


4 


In  1920,  Eienne  Oemichen  built  a  quadrotor  machine  with  eight  additional  rotors  for  control 
and  propulsion.  To  offset  the  overweight,  he  used  a  hydrogen  balloon  to  provide  additional  lift 
and  stability.  In  1924  he  made  a  successful  flight  without  the  balloon  [14]. 

Conventional  UAVs 

Conventional  UAVs  are  the  most  used  in  military  missions,  some  of  which  are  illustrated  in 
Figure  1.1.  These  unmanned  systems  have  been  used  in  many  missions  lately,  and  have  demon¬ 
strated  that  they  can  be  very  reliable  and  more  efficient  than  other  manned  aircraft. 


Figure  1.1:  Conventional  UAV 
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Non-Conventional  UAVs 


Even  though  non-conventional  UAV  systems  are  not  used  in  current  military  contexts,  they  still 
have  a  role  to  play  in  many  current  scientific  research  efforts.  Their  low  speed  compared  to 
conventional  UAV  represent  an  handicap  for  any  military  mission,  however,  nano-UAV’s  have 
the  potential  to  be  useful  in  spying  missions. 
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Figure  1.2:  Non-Conventional  UAV 
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1.2.3  Why  The  Quadrotor? 

As  previously  mentioned,  the  usefulness  of  unmanned  vehicles  stems  from  the  ability  to  avoid 
placing  human  life  in  dangerous  situations.  Modem  UAVs  are  controlled  by  both  autopilots  and 
human  controllers  in  ground  stations.  These  characteristics  allow  them  to  fly  either  under  their 
own  control  or  to  be  remotely  operated.  Rotor-wing  aircraft  grant  some  major  advantages  over 
fixed-wing  aircraft  because  of  their  ability  to  be  launched  without  requiring  a  runway  and  also 
their  ability  to  hover  in  a  given  place.  A  recent  result  reported  in  [15]  is  a  13.6  cm  helicopter 
capable  of  hovering  for  three  minutes,  demonstrating  the  miniaturization  of  unmanned  aerial 
vehicles.  Compared  to  fixed-wing  aircraft,  the  quadrotor  can  be  commanded  to  go  anywhere 
and  examine  targets  in  unlimited  detail.  That  is  why  this  kind  of  UAV  is  considered  ideal  for 
many  information  gathering  applications  in  military  contexts. 

The  primary  concession  of  quadrotors  is  their  relatively  short  endurance.  Operational  time  is 
restricted  to  less  than  half  an  hour  and,  because  of  payload  limitations,  the  option  to  mount 
a  spare  power  supply  is  not  available.  Although  the  simplicity  of  the  mechanical  design  is 
considered  to  be  an  advantage,  especially  for  maintenance,  it  has  a  drawback  in  terms  of  on- 
station  time  and  mission  duration. 

1.2.4  Contribution 

This  thesis  adds  to  work  done  previously  in  developing  obstacle  avoidance  and  map-building 
algorithms  using  MOOS-IvP  modules.  MOOS-IvP  is  an  open  source  software  architecture  for 
autonomous  systems  that  has  been  used  by  many  students  conducting  research  with  unmanned 
underwater  vehicles  [13].  Development  of  algorithms  for  application  to  different  UAV  plat¬ 
forms  leveraging  existing  controller  and/or  driver  software  will  enhance  the  usability  of  these 
unmanned  systems. 

1.3  Outline  of  Thesis 

The  thesis  consists  of  five  chapters.  The  first  chapter  covers  the  main  idea  behind  this  thesis, 
the  purpose  of  the  study,  and  background  information  about  UAVs  and  their  use.  In  the  second 
chapter,  a  description  of  the  goal  is  given,  and  then  an  overview  of  obstacle  avoidance  and  how 
to  model  map  building  using  grid  occupancy  is  also  discussed.  In  Chapter  III,  an  overview  of 
the  hardware  used  for  this  project  is  described  in  detail,  and  an  introduction  to  MOOS-IvP  and 
its  tools  is  presented. 

The  fourth  chapter  covers  details  on  design  of  the  experiments,  the  results  obtained  from  both 
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simulation  and  real-world  implementations,  and  techniques  used  to  perform  the  task.  An  algo¬ 
rithm  for  obstacles  avoidance  and  map  building  is  proposed.  The  last  chapter  summarizes  the 
results,  pointing  out  possible  future  work. 
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CHAPTER  2: 
Formulation 


2.1  Mission  Description 

There  are  several  well  known  approaches  for  obstacle  avoidance  and  path  planning.  Most  of 
the  proposed  techniques  are  suitable  for  outdoor  operation,  while  only  a  few  of  them  have 
been  designed  for  indoor  environments.  For  our  project,  we  are  more  interested  in  developing 
algorithms  for  autonomously  flying  a  quadrotor  inside  the  Naval  Postgraduate  School’s  Center 
for  Autonomous  Systems  (Figure  2.1).  This  will  be  done  by  adapting  techniques  which  have 
been  successfully  tested  on  Underwater  Unmanned  Vehicles. 


Figure  2.1:  NPS  Center  for  Autonomous  Vehicle  Research  (CAVR) 


Obstacle  avoidance,  which  refers  to  the  methodologies  of  shaping  the  robot’s  path  to  overcome 
unexpected  obstacles,  is  one  of  the  most  challenging  issues  to  the  successful  application  of  robot 
systems. 

To  avoid  obstacles  during  a  given  mission,  we  first  need  to  know  where  these  obstacles  are 
located,  and  we  then  avoid  them  by  combining  sensorial  data  and  motion  control  strategies.  A 
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common  strategy  used  for  such  problems  is  to  build  a  map  of  the  robot’s  environment  using 
an  Occupancy  Grid  algorithm,  and  then  apply  a  potential  field  technique  to  drive  the  quadrotor 
from  a  starting  point  to  the  goal.  These  two  approaches  will  be  discussed  in  the  following 
sections. 

2.2  Mapping  Using  Occupancy  Grids 

2.2.1  Basic  Sensor  Model 

The  occupancy  grid  known  as  Evidence  Grid-Based  Map  building  was  introduced  by  Moravec 
and  Elfes  [16],  [17].  A  two  dimensional  grid  is  so  far  the  most  used  in  the  field  of  mobile 
robotics,  and  each  square  of  the  grid  is  called  a  cell.  Each  individual  cell  represents  the  status 
of  the  working  space,  which  can  either  be  empty  or  occupied  by  the  obstacle.  Such  maps  are 
very  useful  for  robotic  applications.  Robots  need  to  know  their  environment  in  order  to  perform 
a  specific  task,  such  as  navigation,  path  planning,  localization  and  collision  avoidance  [18]. 

For  a  given  range-finding  sensor,  such  as  a  laser  line  scanner,  with  maximum  range  R  and 
sensor  beam  half- width  /3,  the  model  can  be  decomposed  into  a  number  of  sectors  labeled  I- IV, 
as  illustrated  in  Figure  2.2  and  presented  by  [19].  . 


Figure  2.2:  The  regions  of  space  observed  by  an  ultrasonic  sensor.  From:  [19] 


A  model  for  the  occupancy  of  cells  in  a  grid  is  Region  I  is  the  region  associated  with  the  laser 
reading.  Region  II  represents  an  empty  area  where  nothing  is  detected  by  the  range  reading. 
Region  III  is  the  area  covered  by  the  laser  beam,  but  remain  unknown  whether  it  is  occupied 
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or  not  because  of  the  occlusion  described  in  chapter  IV.  Finally,  Region  IV  is  outside  the  beam 
area  and  does  not  represent  any  interest. 

For  a  given  measurement  reading  a  range  of  s,  the  given  sensor  model  provides  probabilistic 
expressions  for  reasoning  about  the  occupancy  of  each  cell  in  the  occupancy  map.  As  illustrated 
in  Figure  2.3,  the  relevant  parameters  for  reasoning  about  the  highlighted  (black  outlined)  cell 
include  r,  which  is  the  distance  of  the  grid  element  from  the  sensor  location,  and  a,  the  angle 
of  the  grid  element  relative  to  the  sensor  center  beam. 


Figure  2.3:  Obstacle  Represented  in  a  Grid 


For  a  measurement  s  that  fall  in  region  /,  the  likelihood  that  the  measurement  was  due  to  an 
actual  obstacle  present  at  that  range  and  its  complement  can  be  computed  by: 


P(s  |  Occupied)  = 


*  Max , 


occupied 


P(^|Empty)  =  1  —P  (s  |  Occupied) 


where  Maxoc cupied  is  due  to  the  assumption  that  a  reading  of  “Occupied”  is  never  completely 
reliable  [19]. 

Similarly,  for  a  range  measurement  in  region  II,  the  probabilities  can  be  computed  according  to 


11 


the  model: 


/>(sj  Empty) 


(¥)  +  (^)/5 
2 


P^lOccupied)  —  1  —  /'(■s]  Empty) 


Because  of  the  occlusion,  we  assume  that  the  probability  of  occupancy  in  region  III  is  50% 
occupied  and  50%  unoccupied.  Region  IV  is  not  of  interest. 


2.2.2  Theoretical  Review  of  Bayesian  Rules 

Let  H  =  {Occupied,  Empty}  be  a  binary  random  variable.  The  probability  of  H  is  0  <= 
P(H)  <=  1.  If  P(H )  is  known,  its  complement,  P(notH),  also  denoted  P{ -■//),  can  be  com¬ 
puted  simply  by  P(^H)  =  1  —  P(H). 

These  probabilities  only  provide  us  with  prior  information,  and  they  are  assumed  independent 
from  the  sensor  reading.  For  the  robot  we  need  a  function  to  compute  the  probability  of  elements 
in  the  map  (i.e.,  at  location  [i]  [j]  in  the  grid)  based  on  the  sensor  reading(s)  and  whether  the 
state  of  this  element  is  either  “Occupied”  or  “Empty.”  Then  we  can  compute  P(H\s),  which 
represents  the  probability  that  the  hypothesis  (either  “Occupied”  or  “Empty”)  given  a  particular 
sensor  reading(s). 

Note,  however,  the  sensor  model  provides  only  the  likelihood  probability  P(s\H).  To  derive  the 
posterior  probability  P(H\s),  we  use  Bayes’  rule  [20].  As  a  review,  if  we  consider  A  and  B  as 
two  events,  such  that  P{A)  >  0  and  P(B)  >  0,  then  P(A\B)  is  the  conditional  probability  of  event 
A  given  event  B,  such  that: 

nm  =  (2.D 

where  P(Af]B)  is  the  probability  of  intersection  set  of  A  and  B.  also  known  as  the  joint  proba¬ 
bility  of  A  and  B. 

From  Equation  2.1  we  can  write: 

p(ApB)  =P{A\B)P{B)  =P(B\A)P{A) 
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The  law  of  total  probability  is  given  by  : 


P(A)  =  Y,P(Bi)P(A\Bi)  (2.2) 

iel 

where  P(Bi)  >  0,  and  B,nB/  —  0  for  (i  ^  j,  i,j  e  I  C  {1,2,3,  ...,n}),  where  n  is  the  cardinality 
of  set  I. 


Recalling  the  set  union  identity,  A  =  (J/e/(A  UR),  the  total  probability  (Equation  2.2)  generalized 
from  the  equations  above  can  be  expressed  as: 


Z<e,P(B,)P(A\B,) 


(2.3) 


where  P(Bj\A)  is  called  the  posterior  probability,  P(A\Bj)  is  called  the  likelihood,  and  P(Bj)  is 
the  prior  probability.  The  denominator  term,  £i-e/R(RI-)P(A|R,-)  is  also  given  a  name,  namely 
the  evidence. 


Thus,  it  is  now  possible  to  compute  the  posterior  probability,  that  is,  P(H\s),  using  the  sensor 
likelihood  probability,  P(s\H),  via  Bayes’  rule: 


P(H\s) 


P(s\H)P(H) 

P(H)P(s\H)+P(-^H)P(s\^H) 


For  example,  if  the  hypothesis  H  in  question  is  “Occupied,”  the  above  expression  becomes: 


P(  Occupied  1 5) 


P(s  |  Occupied)^  ( Occupied) 
P(Occupied)P(5|Occupied)  +P(Empty)P(.y|Empty)  ’ 


2.2.3  Updating  the  Occupancy  Map  with  Bayes’  Rule 


Initially,  all  the  grid  map  cells  have  to  be  initialized  with  a  probability  of  0.5  to  reflect  un¬ 
certainty  in  occupancy  of  the  cells.  When  we  get  an  observation  from  the  sensor,  the  new 
probabilities  are  computed  according  to  Bayes’  Rule,  and  the  prior  probability  P(H)  is  replaced 
with  the  new  posterior  probability  value.  Values  above  0.5  indicate  that  cells  are  probably  oc¬ 
cupied;  otherwise  cells  are  probably  empty.  Every  time  the  sensor  platform  moves,  the  grid 
cells  within  the  occupancy  map  are  continuously  updated  with  values  computed  using  the  new 
measurements  from  the  sensor. 
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Even  though  the  Bayesian  update  approach  is  considered  to  be  more  accurate  than  perhaps  its 
more  naive  binary  sensor  models,  the  computational  cost  for  map  building  is  very  high,  which 
is  worsened  by  a  higher  frequency  of  updating  [21].  To  avoid  such  problem,  it  is  recommended 
not  to  do  the  computation  on  the  onboard  PC. 


2.3  Potential  Field  Navigation 

2.3.1  General  structure 

The  artificial  potential  fields  for  a  robot  were  first  proposed  by  Khatib  [22]  and  has  been  studied 
extensively  since  its  inception.  In  this  formulation  where  the  mobile  robot  seeks  to  navigate  to 
a  goal  location,  the  robot  is  considered  a  particle  in  the  configuration  space  which  is  subjected 
to  an  artificial  potential  field  U(q),  where  q  is  the  (vector)  state  of  the  robot  (typically,  for  a 
mobile  point  robot  in  the  plane,  q  =  (jt.y) 7 ).  At  each  iteration,  the  artificial  force  F(q)  induced 
by  the  potential  field  indicates  the  most  promising  direction.  The  following  equations  follow 
the  notation  and  construction  presented  in  [23]. 

The  potential  field  is  defined  as  the  sum  of  an  attractive  potential  UaU  pushing  the  robot  toward 
the  goal  and  a  repulsive  potential  Urep  pushing  the  robot  away  from  obstacles. 

U(q)  =  Uatt(q)  +  Urep(q).  (2.4) 

Consider  the  force  vector  experienced  by  the  particle  to  be  the  negative  gradient  (i.e.,  direction 
of  steepest  descent)  of  the  potential  field. 


F(q) 


—VC  (q)  = 

fdU  dU\T 


— vc/aff  —  WUrep 

,  forqeM2, 

q 


where  VCis  the  gradient  vector  of  U  evaluated  at  the  robot  position.  Equivalently,  the  force  is 
defined  as  the  sum  of  the  two  attractive  and  repulsive  force  vectors,  Fatt  and  Frep,  respectively, 
i.e., 

F{q)  =  Fattiq)  +  Frep(q).  (2.5) 
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2.3.2  Attractive  Potential  Field 


The  attractive  field  may  be  simply  defined  as  a  parabolic  form: 

Uatt(q)  =  ^Cp2goal(q)  (2.6) 

with  £  a  positive  scalar  and  p  goal(^)  is  the  distance  from  q  to  goal. 

The  function  Uatt  is  assumed  nonnegative  and  reaches  its  minimum  at  qgoai  where  V Uatt{qgoai )  = 
0.  Since  Fatt  is  differentiable  everywhere  in  the  configuration  space,  the  attractive  force  can  be 
defined  as  the  following: 


Fattiq)  =  -VUatt{q)  =  -  t;p  goal  {q)V  p  goal  {q)  =  -£(q-qgoai) 


Another  possible  form  of  Uatt  is  conical: 

Uatt(q)  =  ^pgoal(q)  (2.7) 

In  this  case,  we  will  have  then  a  force  of  type: 

Uatt(q)=HvPgoal(q )  =  (2-8) 

The  advantage  of  the  conical  shape  is  that  the  force  is  constant  on  the  space  configuration. 
Moreover,  it  does  not  tend  to  infinity  when  the  robot  moves  away  from  the  goal.  However,  it  is 
not  null  at  the  goal  location,  qsoaj. 

The  most  commonly  used  form  of  potential  field  function  proposed  by  Khatib,  which  is  known 
by  the  gradient  form,  is  defined  as  the  following: 


Uatt(q)^l-^d2  (2.9) 

where  d  =  \q  —  qatt\.  Recall  that  q  is  the  current  position  of  the  robot,  qatt  is  the  position  of 
an  attraction  point  (e.g.,  the  goal),  and  £  is  an  adjustable  scaling  constant.  The  corresponding 
force  function  is  : 

Fattiq)  =  -VUatt  =  -C(q-qo)  (2.10) 
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2.3.3  Repulsive  Potential  Field 

The  repulsive  potential  is  used  to  create  a  potential  boundary  around  obstacles  that  cannot  be 
crossed  by  the  robot.  In  addition,  we  do  not  want  this  potential  to  affect  the  movement  of  the 
robot  when  it  is  sufficiently  far  from  the  obstacles.  We  denote  d  =  \q  —  q0\  to  represent  the 
distance  between  the  robot  and  the  obstacle  in  question.  The  formula  proposed  by  Khatib  is 
given: 


U rep  (d  )  — 


0  if  d  >  do, 


where  q  is  the  robot  position  and  q0  is  the  obstacle  position.  Recall  that  do  is  the  influence 
distance  of  the  force,  and  rj  is  an  adjustable  constant. 


So  then  we  have  the  corresponding  repulsive  force  function: 


Frepi*!) 


f  n  fi-±)  (9-9o) 

J  r]  \  d  do)  di 

\  o 


if  d  <  do, 
if  d  >  do, 


2.3.4  Path  Planning 

After  defining  the  potential  forces,  it  is  sufficient  to  perform  gradient  descent  to  reach  the  goal. 
The  gradient  descent  consists  simply  of  following  the  direction  indicated  by  the  force  F,  by 
moving  in  this  direction  with  one  step  of  lenght  <5,.  For  example,  with  q=(x,y),  we  have: 


x(di+ 1) 


x(qi)  +  8i 


dU 

dx(x,y) 


(2.11) 


y{qi+\)=y{qi)  +  8i^- — r  (2.12) 

dy{x,y) 

If  we  have  a  path  to  the  goal,  when  we  reach  a  distance  less  than  <5,  from  the  qgoa/ ,  the  algorithm 
should  stop  and  return  ’’goal  reached.” 
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CHAPTER  3: 
Hardware  Experiments 


3.1  Equipment 

3.1.1  Quadrotor 

The  following  picture  shows  the  prototype  UAV  control  system  developed  in  this  thesis. 


Figure  3.1:  Quadrotor  Architecture 


In  this  project,  we  use  the  commercially  available  Ascending  Technology  Pelican  quadrotor. 
The  Pelican  consists  of  a  well-modeled  platform  with  four  rotors.  In  order  to  eliminate  the  need 
for  a  tail  rotor,  like  in  standard  helicopter  structure,  the  rotors  were  designed  to  rotate  in  a  cross 
configuration.  In  other  words,  the  front  and  rear  rotors  rotate  counter-clockwise,  while  the  left 
and  the  right  ones  rotate  clockwise,  as  shown  in  Figure  3.2.  Moreover,  to  give  more  stability  to 
the  quadrotor,  the  rotors  have  fixed-pitch  blades  and  their  air  flows  are  oriented  downwards.  The 
quadrotor  was  chosen  because  it  is  considerably  easier  to  manage  than  conventional  helicopters. 
Another  advantage  of  using  quadrotor  design  is  that  they  are  typically  motorized  by  batteries 
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instead  of  a  gas  engine,  which  is  convenient  for  certain  missions  where  noise  caused  by  the 
engine  can  decrease  the  chances  of  surprising  the  enemy.  However,  as  a  drawback,  batteries 
allow  shorter  flying  time.  Nevertheless,  the  quadrotors  are  petite  in  size,  can  be  used  for  indoors 
experiments,  and  are  a  valuable  research  and  potentially  tactical  platform. 


Figure  3.2:  Quadrotor's  motions  description.  From:  [24] 


The  following  paragraph  describes  how  to  compute  the  variables  roll,  pitch  and  yaw. 

Let  Vj  and  7]  be  the  torque  and  thrust  for  z'th  rotor,  and  /  denotes  the  distance  of  the  rotor  from 
the  center  of  mass.  Then  the  total  thrust,  denoted  u\,  is  given  by: 


U\  —  (T\  +  T2  +  ?3  +  T4) 


(3.1) 


In  addition  to  the  total  thrust,  the  rotation  angles  of  roll,  pitch,  and  yaw  can  also  be  controlled 
by  inputs  M2,  M3,  and  M4,  respectively,  using  differential  thrust: 

•  The  roll  angle,  (j),  is  achieved  by  increasing  or  decreasing  the  left  and  right  rotor  speed 
via  control  M2  =  /(T4  —  T$) 

•  The  pitch  angle,  0,  is  provided  by  increasing  or  decreasing  the  front  and  back  rotor  speeds 
via  control  M3  —  1{T\  —  7)) 

•  The  yaw  angle,  \jf,  is  obtained  from  the  torque  resulting  from  the  subtraction  of  the 
counter-clockwise  (front  and  back)  from  the  clockwise  (left  and  right)  speeds,  i.e.,  with 
control  input  M4  =  (M3  +  M4  —  u\  —  M2). 

Table  3.1  summarizes  some  global  characteristics  of  the  Pelican  platform. 
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Flight  Performance 

General  and  Technical  Data 

Cruise  speed 

36  km/h 

Sensor  board  (platform) 

AutoPilot  V2 

Max.  speed 

50  km/h 

Dimensions 

50cm  x  50cm  x  20cm 

Max.  wind  speed 

36  km/h 

Weight  of  vehicle  (ready  to  fly) 

750  g 

Usual  operating  altitude 

50  m 

Number  of  propellers 

4 

Max.  payload(g) 

500g 

Propeller  size 

10  in 

Max.  flight  time 

25  min 

Max  flight  time  at  max  payload 

12-15  min 

Launch  Type 

VTOL 

LiPo  Battery  voltage,  capacity 

11.1  V  (6000  mAh) 

GPS,  Compass 

Included 

Programmable  processor 

Included 

Safe  landing  mode 

include 

Serial  port  interface 

Included 

Table  3.1:  Ascending  Technologies  Pelican  quadrotor  specifications  [25] 


3.1.2  Laser  Scanner 


Figure  3.3:  Hokuyo  URG-04LX.  From:  [26] 

For  path  planning  and  obstacle  detection  within  an  unknown  working  space,  laser  scanners 
are  a  commonly  used  solution  for  mobile  robotic  applications.  Most  previous  work  in  the 
robots  field  has  been  done  with  laser  scanners,  including  those  manufactured  by  Hokuyo.  The 
Hokuyo  lasers  are  relatively  expensive  compared  to  other  sensors,  but  given  their  weight,  power, 
resolution,  and  integration  benefits,  these  small  sensors  are  significantly  easier  to  connect  and 
to  mount  on  the  quadrotor  platform. 

For  this  work,  a  Hokuyo  URG-04LX  unit  is  used  for  area  scanning.  The  light  source  of  the 
sensor  is  an  infrared  laser  of  wavelength  785nm  with  a  Class  1  laser  safety  rating.  The  scan 
area  is  a  240°  field  of  view  with  maximum  sensing  range  of  4m.  The  resolution  of  the  scanner 
is  360°/1024  ticks  which  is  approximately  0.36°  such  that  the  total  number  of  scan  points  is 
683.  Therefore,  if  we  multiply  the  number  of  intervals  (683-1)  by  the  angular  resolution  (0.36°) 
we  get  240°.  Table  3.2  summarizes  some  general  characteristics  of  the  URG-04LX. 


19 


Specifications 

Voltage 

5.0  V  ±  5  % 

Current 

0.5  A  (Rush  current  0.8  A) 

Detection  Range 

0.02  m  to  approximately  4  m 

Laser  wavelength 

785  nm,  Class  1 

Scan  angle 

240° 

Scan  time 

100  ms/scan  (10.0  Hz) 

Resolution 

1  mm 

Weight 

141  gm  (5.0  oz) 

Interface 

USB  2.0,  RS232 

Angular  Resolution 

0.36° 

Table  3.2:  Hokuyo  URG-04LX  Lase  specifications  [26] 


The  URG-04LX  laser  scanner  comes  with  a  library  of  software  written  in  C  and  C++  that 
provide  the  ability  to  communicate  and  capture  data  from  the  sensor.  For  this  thesis,  the  source 
code  was  modified  to  suit  the  logging  and  analysis  needs  of  this  project,  and  can  be  found  in 
Appendix  A. 

The  following  picture  shows  sample  data  that  was  captured  in  the  Autonomous  Systems  Lab. 


Caption: 

•  0  is  max  range. 

•  non  zero  (e.g.,  2.789)  means  obstacle  at  range  in  meters. 

•  N  is  total  number  of  scan  points. 

The  data  returned  from  the  laser  range  finding  sensor  consists  of  an  array  of  tuples  whose 
elements  are  beam  intensity  values  and  range  values  for  a  single  scan.  These  scans  occur  at  a 
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frequency  of  10  Hz.  Also,  for  a  given  beam,  if  no  obstacle  is  detected  within  the  maximum 
sensor  range,  that  scan  point  is  given  a  value  of  zero,  which  can  be  used  for  filtering  the  data 
during  processing.  During  each  of  these  scans,  the  UAV  position  must  also  be  recorded  to 
recover  and  register  the  location  of  obstacles  in  the  global  coordinate  frame  versus  the  local 
(body-fixed)  relative  coordinate  frame.  With  a  known  UAV  pose  (i.e.,  position  and  orientation) 
and  the  laser  scan  data,  a  digital  surface  model  (DSM)  or  terrain  map  can  also  be  obtained,  e.g., 
see  Figure  3.5,  which  has  been  studied  in  previous  works.  Looking  at  the  figure  above,  the  laser 
data  can  be  transformed  to  the  global  frame  using  the  following  equations: 


x  —  xr  +  p  *  cos(0) 
y  =  y>- 

Z  =  Zr-p  *sin(0) 

where  p  is  the  distance  measured  by  the  laser  scanner  and  0  is  the  angle.  The  robot,  or  UAV, 
position  is  denoted  by  xr,  >y,  and  z,r.  Using  these  relationships,  a  plot  showing  the  DSM  can  be 
obtained. 


Figure  3.5:  Flow  to  read  Data  from  Hokuyo 
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3.1.3  Digi  XBee  Wireless  RF  Modules 

XBee  and  XBee-PRO  802.15.4  OEM  RF  modules,  illustrated  in  Figure  3.6  are  embedded  so¬ 
lutions  providing  wireless  end-point  connectivity  to  devices.  These  modules  use  the  IEEE 
802.15.4  networking  protocol  for  fast  point-to-multipoint  or  peer-to-peer  networking.  They 
are  designed  for  high-throughput  applications  requiring  low  latency  and  predictable  communi¬ 
cation  timing  [27]. 


Figure  3.6:  The  Xbee  RF  module  used  in  this  project  [27]. 

XBee  are  very  small  in  size  and  can  be  used  for  any  project  where  we  need  to  interconnect 
and  communicate  between  components  via  wireless  network.  The  XBee  models  are  known  to 
have  limited  input  and  output  pins.  Moreover,  the  XBee  units  does  not  support  analog  output 
or  allow  access  to  other  integrated  capabilities.  For  example,  if  we  want  to  control  the  speed 
of  a  motor,  we  need  to  have  additional  electronic  components,  e.g.,  a  pulse-width  modulation 
breakout  board. 

Though  this  limitation  prevents  standalone  operations  for  embedded  systems,  for  the  purpose 
of  this  study,  the  required  capability  is  to  be  able  to  communicate  between  the  onboard  sensor 
on  the  quadrotor  and  the  laptop  which  is  running  most  of  the  code. 
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Specifications 

Indoor/Urban  Range 

Up  to  300  ft.  (90  m),  up  to  200  ft  (60  m)  int’l  variant 

Outdoor  RF  line-of-sight  Range 

Up  to  1  mile  (1600  m),  up  to  2500  ft  (750  m)  int’l  variant 

Supported  Network  Topologies 

Point-to-point,  Point-to-multipoint,  Peer-to-peer,  and  Mesh 

Operating  Frequency  Band 

ISM  2.4  GHz 

Table  3.3:  XBee-PRO  Serial  RF  (802.15.4)  module  -  Technical  Specifications  [27] 


3.1.4  Ubisense  Location  Solution 

Where  GPS  does  not  work,  such  as  inside  structures  or  tunnel  environments,  alternate  methods 
for  obtaining  robot  positioning  is  necessary,  as  knowledge  of  the  robot  location  (and  orientation) 
are  essential  to  constructing  maps  and  navigation  within  them.  One  technology  available  in  the 
Unmanned  Systems  Laboratory  (see  Figure  2.1)  is  the  Ubisense  system,  which  uses  radio  fre¬ 
quency  beacons  and  sensors  to  provide  position  information.  Characterization  of  the  Ubisense 
position  estimates  and  tracking  performance  is  beyond  the  scope  of  this  thesis,  but  the  reader  is 
referred  to  ongoing  efforts  to  evaluate  this  system  for  use  with  quadrotor  and  other  mobile  robot 
platforms. 


Figure  3.7:  Ubisense.  From  [28] 

Ubisense  uses  a  Combined  Angle-of-Arrival  and  Time-Difference-of-Arrival  approach,  imple¬ 
mented  in  proprietary  software.  In  order  to  have  a  higher  accuracy,  Ubisense  gathers  more 
readings  from  each  sensor  (depicted  in  Figure  3.7)  and  allows  for  a  graphical  display  and  the 
simulation  of  the  effects  of  configuration  changes  [28].  Though  not  explicitly  utilized  in  this 
thesis,  the  Ubisense  hardware  may  be  a  useful  tool  for  providing  robot  localization  information 
in  future  experiments. 
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3.2  Software 

The  simulation  of  both  the  hardware  and  the  algorithms  necessary  for  navigation  and  mapping 
was  developed  in  MATLAB  (as  described  further  in  Chapter  4),  with  the  intention  of  making 
use  of  the  iMatlab  interface  [30]  to  MOOS.  The  implementation  of  the  models  described  in 
this  thesis  are  included  in  the  appendices1.  The  remainder  of  this  section  provides  an  overview 
of  the  MOOS-IvP  autonomy  software  architecture  for  eventual  integration  with  the  presented 
models. 

3.2.1  Mission  Oriented  Operating  Suite 

The  Mission  Oriented  Operating  Suite  (MOOS)  [29]  was  developed  at  MIT  by  Paul  Newman  in 
2001.  It  comprises  open  source  C++  modules  that  provides  autonomy  for  a  number  of  robotic 
platforms.  Originally,  MOOS  was  developed  to  support  operations  for  autonomous  marine 
vehicles,  and  its  first  use  was  for  the  Bluen  Odyssey  III  vehicle  owned  by  MIT. 

The  following  schematic  presents  a  typical  MOOS  communication  configuration  proposed  by 
Newman  [29]. 
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Figure  3.8:  A  Typical  MOOS  Communication  Setup  proposed  by  Paul  Newman  [30] 


'The  associated  software  can  also  be  found  online  at  http :  //faculty .  nps  .  edu/thchung. 
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Note  the  module  naming  convention,  where  a  module  preceded  by  a  “p”  means  it  is  a  process, 
and  one  preceded  by  an  “i”  represents  that  it  is  associated  with  an  instrument.  Because  most  of 
the  modules  shown  in  this  graph  were  developed  to  work  with  unmanned  underwater  vehicles, 
we  will  not  use  them  for  this  project. 

For  our  project,  however,  MOOS  is  utilized  to  implement  a  set  of  low  level  functions  required 
for  a  quadrotor  to  fly.  One  advantage  of  MOOS  is  its  subscription-based  database  system.  In 
other  words,  users  can  run  many  processes  with  only  one  message  interface.  Additionally, 
MOOS  is  a  modular  architecture  which  makes  it  easier  for  programming  and  updating  the 
project  by  adding  other  pieces  of  code.  For  a  multi-group  project,  members  can  work  inde¬ 
pendently  without  even  knowing  what  the  others  are  working  on.  Instead,  additional  modules 
relevant  to  the  project  include  software  to  interface  with  the  quadrotor  and  laser  sensor  as  well 
as  the  algorithms  for  potential  field  navigation  and  occupancy  grid-based  mapping.  Figure  3.9 
illustrates  the  required  modules  for  our  navigation  and  mapping  project. 


pAntier 
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Figure  3.9:  MOOS  Communication  Setup  Needed  for  the  Project 

Another  advantage  of  MOOS  is  that  the  software  comes  with  a  set  of  mission  parameters  in 
a  standard  format.  This  enables  users  to  just  change  out  settings  according  to  their  projects 
without  needing  to  rewrite  every  function.  After  we  have  made  all  the  necessary  changes  and 
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added  the  new  modules,  MOOS  can  be  rebuilt  simply  by  executing  the  build-moos .  sh  script. 


3.2.2  Interval  Programming,  IvP 

Interval  Programming  (IvP)  is  a  mathematical  programming  model  for  multi-objective  opti¬ 
mization  [31].  In  the  IvP  model,  each  objective  function  is  a  piecewise  linear  function.  The 
IvP  model  and  algorithms  are  included  in  the  IvP  Helm  software  as  the  method  for  representing 
and  reconciling  the  output  of  helm  behaviors.  The  term  interval  programming  was  inspired  by 
the  mathematical  programming  models  of  linear  programming  (LP)  and  integer  programming 
(IP).  The  pseudo-acronym  IvP  was  chosen  simply  in  this  spirit  (and  to  avoid  acronym  clash¬ 
ing).  For  more  information  on  how  to  use  MOOS-IvP,  the  reader  is  referred  to  the  MOOS-IvP 
documentation  [31]. 
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CHAPTER  4: 

Design,  Analysis,  and  Results 


4.1  Methods 

The  broad  scope  of  this  project  involved  both  hardware  and  software  development,  and  this 
thesis  represents  initial  efforts  to  implement  and  integration  the  various  system  components. 

4.1.1  Architecture  Design 

Because  of  the  rapid  development  time  frame,  it  was  easier  to  prototype  the  implementation  of 
the  hardware  using  MATLAB  as  a  simulator  and  interface  for  the  experiments  instead  of  using 
simulation  from  the  MOOS-IvP  software.  MATLAB  also  contains  common  linear  algebra  and 
image-processing  functions  which  helped  to  develop  test  cases  and  evaluate  results  with  much 
less  effort.  With  MATLAB  we  can  quickly  generate  plots  and  visualize  the  simulations  to  help 
us  understand  the  behavior  of  the  quadrotor  in  a  given  scenario.  Using  MATLAB  exclusively 
throughout  the  project,  may  not  be  recommended  for  operational  settings,  as  it  is  a  higher  level 
scripting  language  which  has  potential  performance  limitations,  e.g.,  slow  runtime  speeds,  and 
potential  hardware  interface  challenges. 

Figure  4.1describes  the  designed  hardware  and  software  architecture,  for  which  this  work  has 
prepared  the  foundation.  The  color-coded  links  between  boxes  represents  how  hardware  com¬ 
ponents  are  going  to  communicate  with  each  other  and  with  the  software  modules. 
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- ►  Wired  - ►  WiFi  - ►  Software 

_ The  MOSS  modules  inside  this  box  cannot  be  added  to  the  Quadrotor's  software 


Figure  4.1:  Proposed  hardware  and  software  architecture  for  navigation  and  mapping  using  the  laser 
line  scanner  and  the  quadrotor  UAV. 

4.1.2  Wireless  Communication  Integration  Tests 

In  order  to  ensure  that  the  XBee  wireless  modules  are  working,  a  simple  test  is  to  send  a  chunk 
of  data  from  both  sides  of  the  wireless  channel.  If  the  transmitted  and  received  data  are  the 
same,  with  no  extra  letters  or  digits  added,  then  a  successful  connection  between  the  two  XBee 
modules  has  been  established. 

If  developing  in  the  UNIX-based  environment,  it  is  recommended  that  you  use  cutecom  soft¬ 
ware1  to  communicate  with  the  devices,  as  it  is  very  straightforward  and  easy  to  use.  The  only 
required  information  is  the  name  of  the  device.  To  do  that,  one  can  simply  execute  the  command 
dmesg  in  a  terminal  window  to  display  the  name  of  the  device  to  which  it  was  attached.  The 

'Available  at  http :  / / cutecom.  sourcef orge  .  net/ 
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following  illustrative  message  gives  an  idea  of  how  to  access  the  name  of  the  serial  port: 


[###]  usb  6-2:  FTDI  USB  Serial  Device  converter  now  attached  to  ttyUSBO 

In  this  case,  in  cutecom  we  should  enter  the  device  name  as  /dev/ttyUSBO. 

If  you  are  using  Windows,  we  recommend  using  HyperTerminal.  As  we  did  in  UNIX,  we  need 
to  know  to  which  serial  communications  port  the  XBee  was  connected.  Under  “My  computer,” 
select  “Manage”  and  then  “Device  Manager.”  You  should  see  under  Ports  something  like  “USB 
Serial  Port(COM4) .”  This  means  that  your  device  was  attached  to  port  COM4.  So  when  you 
use  HyperTerminal,  you  should  choose  COM4  to  open  the  connection  with  the  device.  After 
that,  we  need  to  have  the  same  setup  on  both  sides  (i.e.,  Baud  Rate  and  Parity).  Before  you  can 
send  data,  you  must  make  sure  that  the  connection  between  the  XBees  is  working  perfectly.  We 
first  need  to  configure  the  hardware.  Usually,  the  Xbees  come  with  a  software  called  X-CTU  for 
configuration.  Suppose  you  send  “VVVVVV,”  but  you  receive  “VxobVxobVxob.”  This  means 
that  there  is  a  problem  in  your  setup  or  maybe  a  conflict  with  USB’s  version.  After  you  solve 
this  problem,  you  can  start  sending  your  data  to  the  XBee.  A  simple  shell  script  (provided  in 
Appendix  A)  can  be  used  to  redirect  the  data  from  the  laser  to  the  XBee. 
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4.2  Simulation  Results 

4.2.1  Data  Collection 

To  have  a  better  understanding  of  the  sensor’s  data,  it  was  necessary  to  gather  large  amounts  of 
sensor  data  from  various  situations.  According  to  the  manual  for  the  sensor,  the  maximum  range 
that  can  be  reached  is  about  4  meters,  but  the  experiments  proved  that  5  meters  are  reachable, 
especially  when  we  use  the  sensors  indoors.  Moreover,  windows  represent  a  trap  for  sensors. 
From  a  distance  of  three  meters,  sensors  do  not  get  any  returns  from  a  window.  Even  though 
this  error  can  be  corrected  at  a  closer  distance,  the  results  from  the  potential  field  computations 
will  be  wrong  and  could  result  in  a  waste  of  time  in  navigating  from  a  given  start  point  to  the 
goal. 


Figure  4.2:  Real  Data  From  Sensors 


4.2.2  Occlusion 

Occlusion  is  defined  by  the  shape  of  the  obstacle  and  the  position  of  the  sensor  facing  the 
obstacle  at  that  time.  Occlusion  can  be  a  problem  if  the  mission  consists  of  not  just  finding 
goals,  but  also  building  a  real  map  for  the  environment’s  mission.  To  solve  this  problem,  the 
sensor  has  to  face  the  obstacle  from  all  sides.  Figure  4.3  gives  an  idea  how  the  shapes  can  be 
interpreted  differently. 
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(a) First  reading  (b)  real  obstacle  (c)  real  word 


Figure  4.3:  Real  Data  From  Sensors 


4.2.3  Occupancy  Grid 

The  Occupancy  Grid  algorithm  as  described  in  Chapter  II,  consists  of  updating  the  probability 
of  occupancy  every  time  we  observe  a  hit  from  the  sensor.  The  snapshots  depicted  in  Figure 
4.4  and  Figure  4.5,  taken  from  simulated  data,  show  how  an  obstacle  can  be  detected.  This 
simulation  was  performed  in  a  known  environment  where  the  obstacles  were  plotted  with  values 
equal  to  one.  The  MATLAB  code  can  be  found  in  Annex  B. 


Figure  4.4:  Obstacle  Detection 

As  we  can  see  from  Figure  4.4,  an  area  of  the  grid  darkens  when  an  an  obstacle  is  detected  and 
lightens  when  the  observation  returns  a  low  probability  of  occupancy.  After  several  runs,  the 
map  should  look  like  Figure  4.5. 
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Figure  4.5:  Updating  Grid  Probability 


4.2.4  Potential  Field 

The  MATLAB  picture  shown  in  Figure  4.6  displays  a  potential  field  plot  indicating,  first,  how 
a  quadrotor  can  react  to  repulsion  forces  generated  by  the  obstacles  and,  second,  how  the  at¬ 
traction  forces  push  the  quadrotor  toward  the  goal  even  as  it  avoids  obstacles  encountered  in  its 
path. 


Figure  4.6:  Path  and  Map  Generated  by  the  Potential  Field  Functions 
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4.3  Physical  Robot  Tests 

Most  of  the  experiments  have  been  done  in  the  Autonomous  System  Lab.  The  first  experiment 
was  conducted  by  driving  the  quadrotor  using  a  cart.  This  step  was  very  important  in  evaluating 
the  results  from  the  map-building  process.  This  section  highlights  a  number  of  initial  component 
efforts  in  order  to  prepare  for  integrated  testing. 

4.3.1  Converting  between  Local  and  Global  Coordinate  Frames 

Preliminary  laser  data  processing  tasks  includes  conversion  from  relative  local  coordinates  (due 
to  range  and  bearing  measurements)  of  the  obstacles  to  global  coordinates  in  order  to  ensure  an 
appropriately  rectified  map  reflecting  the  real  world. 

Coordinate  transformations  in  the  plane  require  a  rotation  and  translation  of  the  robot  frame 
to/from  the  global  reference  frame.  Consider  first  the  case  where  the  quadrotor  is  located  at 
(xR.yR)  —  (0,0),  i.e.,  both  the  local  and  global  coordinate  frames  overlap  at  the  origin,  but  with 
some  heading  0,  as  shown  in  Figure  4.7  below. 


Figure  4.7:  Definition  of  parameters  for  transformations  between  rotated  coordinate  frames. 

Given  the  local  robot  coordinate  system,  denoted  by  axes  x,y  (as  opposed  to  the  global  frame 
with  axes  x',y'),  a  laser  range  finder  provides  a  relative  range  and  bearing  measurement,  namely 
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p  and  <f>,  respectively.  The  scan  point  (labeled  point  M  in  Figure  4.7)  represents  a  possible 
obstacle  at  that  location. 

Then,  recalling  the  rotation,  0,  between  frames,  the  coordinates  of  the  scan  point  can  be  repre¬ 
sented  in  both  local  and  global  reference  frames  as: 

x  —  p  cos (0)  (local)  4=F  x'  —  p  cos(0  —  0)  (global) 

y  —  p  sin (<f>)  (local)  4=>  V  —  p  sin(<f)  —  0)  (global) 

As  mentioned  previously,  for  simplicity  in  modeling,  assume  the  sensor  scan  spans  180°  starting 
at  —  §  to  |.  Then  the  angle  to  the  minimum  angle,  i.e.,  the  first  scan  point,  requires  an  offset  of 
—  The  following  examples  show  the  results  for  different  heading  angles  as  special  cases. 


Heading  =  tx/2 

Heading  =  n/4 

Heading  =  3ix/4 

Heading  =  -n/4 

0=heading-Ji/2=O 

0=heading-Ji/2=-Ji/4 

0=heading-n/2=Jt/4 

0=heading-n/2=-  3n/4 

^ 'j 

Figure  4.8:  Coordinate  transformation  of  laser  scan  points  for  different  robot  heading  angles. 


In  the  more  general  case  where  the  quadrotor  is  located  at  a  global  position  x^.y^  with  orien¬ 
tation  0,  the  conversion  between  local  and  global  coordinates  must  account  for  this  translation 
when  computing  the  location  of  the  scan  point  in  question.  In  other  words,  the  following  ex¬ 
pressions  represent  the  appropriate  transformation: 


x  —  p  cos (0)  (local) 
y  —  p  sin (0)  (local) 


x  =  xr  +  p  cos(0  —  0)  (global) 
y  =  yR  +  P  sin(0  -  0 )  (global) 


Figure  4.9  illustrates  an  example  case  with  a  heading  of  0  =  f,  where  again  the  range  and 
bearing  to  the  scan  point  M  are  denoted  p  and  0. 
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Figure  4.9:  Coordinate  transformation  from  local  to  global  coordinates  for  the  special  case  of  a  robot 
located  at  (xg,yg)  =  (posn(l), posn(2))  with  heading  0  = 

4.3.2  Map  Building  and  Potential  Field  Process  Flowchart 

The  graph  below  shows  how  the  MATLAB  scripts  are  executed  and  explains  which  script  is 
called  and  the  order  in  which  the  scripts  are  called.  The  order  of  execution  is  very  important 
because,  for  example,  if  GETDATA  does  not  work  for  some  reason,  then  the  whole  process  will 
fail  to  execute.  The  scripts  were  not  designed  to  catch  errors  and  return  a  detailed  message  to 
help  users.  We  will  leave  this  for  a  future  work. 

4.3.3  How  to  Call  MATLAB  Scripts  from  MOOS 

This  section  describes  the  procedure  for  integrating  algorithms,  e.g.,  those  developed  in  MAT¬ 
LAB,  into  the  MOOS  autonomy  software  architecture. 

Installation  of  MOOS-IvP  software 

The  MOOS-IvP  autonomy  software  is  available  at  http://www.moos-ivp.org,  the  source 
for  which  which  can  be  downloaded  from  a  version-controlled  software  server.  Two  steps 
are  indispensable  to  building  MOOS-IvP,  and  they  have  to  be  executed  in  order.  First,  the 
MOOS  tree  must  be  built  by  executing  build-moos .  sh  script.  Then,  by  executing  the  script 
build-ivp .  sh,  the  IvP  directory  will  be  built  automatically  and  the  project  should  be  ready  to 
run  [31].  The  main  processes  that  must  we  are  concerned  with  are:  On  StartupO,  Connect 
to  Server (),  Iterate (),  and  On  New  Mail(). 
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Running  MATLAB  with  MOOSDB 


MOOS  code  can  be  called  from  inside  MATLAB  using  the  open-source  software  project,  iMat- 
lab  [30].  iMatlab  allows  MATLAB ’s  script  to  connect  the  MOOSDB  either  to  receive  or  to 
send  variables  needed  for  the  project.  To  send  data  from  the  matlab  script,  we  have  to  use  the 
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following  iMatlab  syntax: 


»  iMatlab(M00S  MAIL  TX  ,  VARNAME ,  VARVAL ), 

where,  for  example,  VARNAM i?=pitch,  VARVAL=\5. 

Figure  4.1 1  below  shows  a  typical  configuration  for  iMatlab. 


ProcessConfig 


iMatlab 


AppTick  =  10 

CommsTick  =  10 

Port  =  CDM6 

BaudRate  =  4800 

Verbose  =  false 

Streaming  =  false 

MOOSC  mints  =  true 
SerialComms  =  false 
SERIAL-TIMEOUT  =10.0 
SUBSCRIBE  =  DB.TIME  @0.0 


Figure  4.11:  A  typical  configuration  block  for  iMatlab 


Integrating  Potential  Field  and  Occupancy  Grid  Mapping  Algorithms  into  MOOSDB 

The  concept  of  how  the  potential  field  navigation  and  occupancy  grid  mapping  processes  should 
be  connected  to  MOOSDB  is  illustrated  in  Figure  4.12.  Taking  advantage  of  iMatlab,  the 
MATLAB-based  implementations  presented  in  this  thesis  can  easily  and  transparently  receive 
and  send  data  with  other  processes  running  within  the  MOOS  project. 

When  the  sensor  detects  an  obstacle  in  its  range,  this  data  will  be  used  by  the  occupancy-grid 
process  to  build  the  map  and  also  by  the  potential-field  process  to  compute  the  new  heading.  The 
new  heading  will  be  translated  to  either  yaw,  pitch  or  roll.  When  this  new  value  gets  published, 
the  iAutopilot  process  will  give  an  order  to  the  quadrotor  to  change  direction.  Actually,  the 
check-mail  process  is  in  charge  to  listen  and  detect  any  changes  occurring  during  the  mission. 
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Figure  4.12:  Proposed  configuration  for  MOOSDB  integrating  navigation  and  mapping  algorithms 
via  iMatlab  and  hardware,  e.g.,  laser  scanner  (Hokuyo),  aerial  platform  (quadrotor),  and  localization 
system  (UBisense). 


4.3.4  Results 

Despite  successful  preliminary  investigations  in  simulation,  our  first  attempt  to  build  a  map 
was  not  successful.  A  number  of  error  phenomena  were  observed.  For  example,  upon  further 
analysis,  there  were  repeatedly  relatively  large  offsets  between  the  real  location  of  obstacles  and 
their  location  as  represented  in  the  occupancy  grid-based  map. 

Several  factors  could  have  caused  these  errors.  The  main  factor  was  the  grid  size  and  resolution. 
To  reduce  the  computational  load,  we  fixed  the  size  of  the  grid  to  a  140x140  square  arena 
(i.e.,  19,600  cells),  such  that  each  cell  represented  a  lOcmx  10cm  spatial  block.  Since  the  data 
collected  from  the  sensor  were  on  the  order  of  millimeters  due  to  the  precision  of  the  laser 
sensor,  the  shape  of  the  obstacle  within  the  map  was  limited  by  the  coarse  resolution.  This 
resulted  in  a  representation  that  was  somewhat  different  than  the  shape  of  the  real  obstacle. 

Another  contributing  factor  to  errors  in  the  map  was  the  imperfect  knowledge  of  the  vehicle 
position  and  orientation  at  the  time  we  captured  the  image.  Since  no  positioning  system,  such 
as  the  Ubisense,  was  available,  rough  pose  estimates  were  measured  by  hand  and  aligned  ap¬ 
proximately  with  laser  scan  data. 
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A  remaining  problem  is  related  to  well-known  limitations  of  the  potential  field  algorithm, 
which,  due  to  its  gradient-based  approach,  can  suffer  from  getting  trapped  in  local  minima 
of  the  potential  field.  An  illustration  of  such  a  trapping  scenario  for  potential  field-based  navi¬ 
gation  is  provided  in  Figure  4. 13,  showing  how  a  vehicle  can  get  trapped  and  fail  to  escape  from 
the  local  minimum. 


Figure  4.13:  Illustration  taken  from  [32]  showing  the  pitfalls  of  local  minima  in  potential  field-based 
algorithms  for  mobile  robot  navigation. 

The  first  problem  is  trivial  and  can  be  addressed  by  changing  the  grid  size  or  by  using  software 
other  than  Matlab.  This  will  allow  implementation  of  a  larger  grid  without  being  penalized  in 
runtime  cost. 

Since  we  had  to  simulate  the  coordinates  for  the  quadrator,  instead  of  using  the  real  position 
from  the  Ubisense  (because  the  lack  of  time),  we  cannot  be  sure  that  the  error  we  had  will 
cause  a  real  problem  when  we  use  GPS  or  Ubisense  to  navigate.  We  will  leave  this  for  future 
work  to  be  tested  and  fixed.  The  last  problem  is  very  trivial  and  can  be  addressed  easily.  There 
are  several  approaches  proposed  to  avoid  this  problem.  A  very  simple  implementation  is  to 
set  up  a  series  of  repulsive  forces  with  zero  radius  positioned  at  newly  visited  positions.  As 
time  progresses,  the  repulsive  force  decreases  and  the  recently  visited  locations  become  more 
repulsive  than  positions  visited  a  longer  time  ago.  [33] 
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CHAPTER  5: 
Conclusions 


5.1  Discussions 

This  thesis  presented  an  implementation  study  of  sensor-based  robot  algorithms  for  probabilistic 
mapping  of  an  indoor  environment  and  collision-free  navigation  to  a  goal  within  that  environ¬ 
ment.  The  former  task  was  addressed  by  generating  an  occupancy  grid-based  map  using  models 
of  a  laser  line  scanner  sensor  to  detect  obstacles,  and  both  binary  or  Bayesian  sensor  updates 
to  the  map  were  investigated.  Discretization  of  the  given  area  provided  cells  in  space  which 
reflected  the  presence  or  absence  of  an  obstacle  in  each  cell.  These  types  of  probabilistic  maps 
possess  several  advantages  over  static  counterparts,  including  the  ability  to  refine  the  maps  in 
the  presence  of  uncertainty.  Additionally,  if  obstacles  are  dynamically  moving  in  the  workspace, 
the  use  of  sensor-based  methods  such  as  presented  in  this  thesis  allow  for  appropriately  inter¬ 
preting  newly  obstructed  locations  while  clearing  previously  occupied  ones. 

The  field  of  robot  motion  planning  is  diverse  and  can  be  categorized  coarsely  into  a  few  gen¬ 
eral  approaches,  including  roadmap,  cell  decomposition,  sampling,  and  potential  field  algo¬ 
rithms  [34].  For  this  thesis’  initial  implementation,  we  selected  Artificial  Potential  Field  algo¬ 
rithms.  A  potential  function  is  assigned  to  each  obstacle  detected  using  a  model  of  the  laser 
rangefinding  sensor.  Using  the  computed  force  to  guide  the  motion  of  the  robot,  this  approach 
allows  us  to  derive  a  collision-free  path  to  the  goal.  Though  mathematically  elegant,  a  number 
of  implementation  challenges  were  faced  due  to  the  need  to  address  practical  considerations.  On 
the  other  hand,  it  will  ultimately  be  easier  to  implement  this  algorithm  within  the  MOOS-IvP 
architecture  since  it  straightforwardly  outputs  a  new  desired  heading  which  can  be  translated 
easily  to  pitch,  roll  and/or  yaw  controls  to  be  sent  to  the  quadrotor  platform.  If  the  new  desired 
heading  does  not  deviate  significantly  from  the  previous  one,  then  the  robot  may  just  keep  going 
forward  without  changing  direction.  If  a  new  heading  is  computed,  the  nature  of  the  quadrotor 
platform  allows  for  easy  translation  of  these  deviations  to  motor  commands. 

In  summary,  this  thesis  can  be  considered  an  initial  effort  to  contribute  to  a  larger  systems 
project  involving  the  quadrotor  unmanned  aerial  vehicle.  The  primary  contributions  are  to  ex¬ 
plore  and  implement  common  robot  algorithms  for  building  a  probabilistic  map  using  an  occu¬ 
pancy  grid  approach  and  also  for  avoiding  obstacles  using  a  potential  field  technique.  Parallel 
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efforts  by  other  researchers  have  developed  flight  control  software,  also  in  MATLAB,  capable 
of  sending  and  receiving  information  from  the  quadrotor’s  autopilot.  This  code  can  control  the 
pitch,  yaw,  roll  and  thrust  of  the  quadrotor  executing  a  waypoint  following  mission.  This  thesis 
provides  complementary  and  higher-level  capabilities,  given  the  autonomy  necessary  to  com¬ 
pute  new  headings  based  on  the  navigation  and  mapping  algorithms.  Though  beyond  the  scope 
of  this  investigation,  the  ultimate  objective  is  to  provide  a  robust  integration  of  these  parallel 
capabilities. 

5.2  Future  Work 

There  are  numerous  issues  to  be  addressed  and  various  extensions  to  be  pursued,  but  this  thesis 
can  serve  as  both  an  initial  proof  of  concept  and  a  foundation  for  the  implementation  efforts 
necessary. 

Recall  that  the  main  goal  of  this  thesis  was  to  provide  the  quadrotor  a  method  to  navigate  in  a 
two-dimensional  environment.  However,  given  the  intrinsic  three-dimensional  nature  of  aerial 
robot  platforms,  restriction  to  the  plane  can  be  too  limiting.  For  example,  the  shortest  path 
to  the  goal  may  merit  having  the  quadrotor  fly  above  and  over  an  obstacle  instead  of  driving 
around  it.  Such  tradeoffs  cannot  be  explored  nor  accomplished  if  we  just  limit  our  computation 
to  a  two-dimensional  grid.  My  recommendation  is  to  convert  this  code  to  a  three-dimensional 
structure,  coupled  with  sensor  fusion  algorithms  [35],  to  improve  the  fidelity  of  perception  by 
increasing  the  observation  opportunities  and  operational  realism  of  the  navigation  mission. 

Another  avenue  for  future  work  is  to  improve  the  occupancy  grid-based  mapping  algorithm 
to  reduce  the  computational  burden,  so  as  to  be  executable  on  embedded  systems  onboard  the 
quadrotor.  Enhancements  with  new  data  structures  and  software  engineering  can  leverage  the 
lessons  learned  in  this  thesis  to  realize  this  recommendation. 
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APPENDIX  A: 

C++  CODE  for  the  LASER 


A.l  Modified  C++  code  for  the  Hokuyo  LIDAR  Sensor 

The  driver  for  Hokuyo  (URG)  can  be  downloaded  from  here  [36]. 

/*  ! 

\example  mdCaptureSample  .  cpp 
\  brief  Sample  to  get  data  using  MD  command 
\author  Satofumi  KAMMURA 
\modified  by  Mejdi  Ben  Ardhaoui 

$Id  :  mdCaptureSample  .  cpp  1683  2010—02—10  1 0 : 2 8 : 0 5 Z  satofumi  $ 

*/ 

#include  ’’UrgCtrl.h” 

#include  ’’delay.h” 

#in  elude  ”  ticks,  h” 

#include  <cstdlib> 

# i n elude  <cstdio> 

using  namespace  qrk ; 
using  namespace  std  ; 

//  !  main 

int  main(int  arge  ,  char  *argv[]) 

{ 

# i f de f  WINDOWS A>S 

const  char  device  []  =  ’’COM3”; 

#else 

const  char  device  []  =  ”  /  dev /ttyACMO”  ; 

#endif 

UrgCtrl  urg  ; 

if  (!  urg . connect ( device ) )  { 

printf  ( ’’UrgCtrl  ::  connect  :  %s\n”  ,  urg.what()); 
exit  ( 1 ) ; 

} 

#  i  f  1 

//  Set  to  MD  mode  to  acquire  data 
urg  .  setCaptureMode(  AutoCapture  ) ; 
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#else 


//  Mode  to  get  distance  data  and  intensity  data 
urg  .  setCaptureMode(IntensityCapture  ); 
urg  .  setCaptureSkipLines  (2); 

#endif 

int  scan.msec  =  urg  .  scanMsec  ( ) ; 

#  i  f  0 

//  Set  range  of  acquisition  from  the  center  to  left  90  degree. 

//  Set  range  of  acquistion  from  center  to  right  to  90  degree. 

//  So  In  total  it  will  be  180  degree, 
const  double  rad90  =  90.0  *  MJPI  /  180.0; 

urg  .  setCaptureRange(urg  .  rad2index(— rad90)  ,  urg  .  rad2index(rad90  )); 
#endif 

int  pre_timestamp  =  ticks  (); 

//  Data  is  acquired  continuously  using  MD  command 
//  but  outputs  data  of  specified  number  of  times  . 
enum  {  CaptureTimes  =  1}; 
urg  .  setCaptureTimes  (CaptureTimes  ); 
for  (int  i  =  0;  i  <  CaptureTimes;)  { 
long  timestamp  -  0; 
vector<long>  data  ; 

//  Get  data 

int  n  =  urg  .  capture  (  data  ,  &timestamp); 
if  (n  <=  0)  { 

delay(scan_msec  ); 
continue  ; 

} 

#  if  1 

for  (int  j  =  90;  j  <  n  — 90;  ++j  )  { 

pr  i  n  tf  ( "%4.3  f\n”  ,  data[j]/1000.0); 

//  We  divide  by  1000  to  get  results  in  meters 


} 


#  e  n  d  i  f 
++i  ; 

} 

# i f de f  MSC 
getchar  ( ) ; 
#endif 

return  0; 

} 
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A.2  Script  for  Capturing  Laser  Data 


The  following  script  allows  to  send  the  data  captured  to  the  Xbee  instead  of  sending  it  to  the 
standard  output. 


#  !  /  bin  /  sh 

stty 

-F  / 

dev / ttyUSBO  57600 

while 

[  1 

] 

do 

.  /  mdCaptureSample  > 

/  dev  /  ttyUSBO 

# 

If  we  want  to  keep 

a  copy 

of  data  captured 

we 

# 

have  to  add  the  following 

line  which  save 

the 

# 

last  Laser  reading 

# 

cp  laserData.txt 

/  Laser  /  laser  Data. 

txt 

done 

Some  notes: 

•  The  >  is  used  to  redirect  the  data  captured  from  the  laser  to  the  XBee  attached  to  ttyUSBO 

•  The  argument  57600  means  we  communicate  in  both  transmitting  and  receiving  across 
the  serial  port  with  a  baud  rate  of  57600  bits  per  second 

•  mydatacapture  is  the  name  of  the  modified  laser  driver  code  which  calls  other  programs 
to  extract  data  from  the  laser 
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APPENDIX  B: 

MATLAB  Code  (Occupancy  Grid) 


The  goal  of  this  code  is  to  represent  a  map  of  the  environment  as  an  array  of  cells.  Each 
cell  holds  a  probability  value  which  indicates  if  that  cell  is  occupied  or  not.  All  source  files, 
including  this  MATLAB  code,  is  available  online1. 


%  Created  By  Michael  A.  Goodrich 
%  Modified  By  Mejdi  Ben  Ardhaoui 


LabDim  =  140; 

OBS.SIZE  =  10; 

Quadrotor  =  OBS.SIZE/4; 

Sensor_Range  =  OBS_SIZE; 

NObst  =  6; 

vel  -  4; 

dt  =  0.1; 

ri  =  10;  ci  =  30; 

rf  =  130;  cf  =  120; 

theta  =  atan2(rf— ri  ,  cf— ci); 
Del  =  10; 


%  World  Dimension  (Square) 

%  Obstacles  Size 
%  Quadrotor  Size 
%  Sensor  Range 

%  Number  of  Obstacles  in  this  World 
%  Velocity  in  units  per  sample. 

%  How  often  the  Quadrotor  ’  s  position  is  updated. 
%  Intial  ROW  and  COLUMN  on  the  world  grid 
%  matrix  ,  where  the  path  is  to  begin 
%  Final  ROW  and  COLUMN  on  the  world  grid 
%  matrix  ,  where  the  path  is  to  end 
%  Initial  direction  of  the  Quadrotor 
%  Tolerance  distance  from  the  Goal 


%  Initialize  the  probability  to  0.50 
p  =  .50*  ones  (LabDim  ,  LabDim  ) ; 

%  Plot  the  probability  that  a  grid  location  is  occupied  by  an  obstacle  as 
%  a  pseudo  — intensity  —  darker  colors  indicate  that  the  grid  location  is 
%  more  likely  to  be  occupied. 

GridHandle  =  zeros  (LabDim  ,  LabDim  ) ; 
elf  ;  close  all  ; 
hold  on ; 

axis([0  LabDim  0  LabDim]); 

d  i  s  p  (  ’  Click  to  select  starting  position’); 

[ci,ri]  =  ginput(l); 

%  TODO  check  if  the  start  point  is  within  the  obstacle  boundaries 


'All  source  code  is  available  online  at  http :  //f acuity  .nps  .  edu/thchung  under  Resources ,  Software. 
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text  (  ci  ,  ri  Start  ’  ,  ’FontSize’,  10); 
disp(’Click  to  select  goal  position’); 

[cf,rf]  =  ginput(l); 

%  TODO  check  if  the  start  point  is  within  the  obstacle  boundaries 
text  (  cf  ,  rf  Goal  ’  ,  ’FontSize’,  10); 

%  Re— compute  the  initial  theta 

theta  =  atan2(rf— ri  ,  cf— ci);  %  Initial  direction  of  the  Quadrotor 

for  i  =  l:LabDim 

for  j  =  1 :  LabDim 

GridHandle  ( i  ,  j  )  =  plot  ( i  ,  j  ,  ’  .  ’  ) ; 

set  (  GridHandle  (i  ,j  ),  ’color’,  [1— p(i  ,  j  ),1  —  p(i  ,  j  ),1  —  p(  i  ,j  )]  ,... 
’  MarkerSize  ’  ,5  ,  ’  EraseMode  ’  ,  ’  normal  ’  ) ; 

end  ; 

end  ; 

hold  off ; 

set(gca,’DataAspectRatio’  ,[1  ,1  ,1]); 
axis  ([0 ,140  ,0 ,140]); 


Likelihoods  AND  Definitions 


%  The  two  possible  states  for  each  cell:  occupied  and  unoccupied.  For  each% 


%  cell  ,  there  are  three  possible  observations  :  hit  ,  no— hit  ,  and  no  % 

%  observation  .  We  will  only  use  the  hit  or  miss  observations  .  % 

%  p(0=hit  |  S=occupied )  =  ’’TrueHit”  % 

%  p(0=hit  |  S=unoccupied )  =  ’’FalseAlarm”  % 

%  p(0=no— hit  |  S=occupied  )  =  ” Mi ssedDetection ”  =  1— TrueHit  % 

%  p (0=no— h it  |  S=unoccupied  )  =  ’’TrueMiss”  =  1— FalseAlarm  % 


%The  probability  that  an  occupied  cell  is  detected  as  a  hit  by  the  observer. 
TrueHit  =  0.98; 

%The  probability  that  an  unoccupied  cell  is  detected  as  a  hit. 

FalseAlarm  =  0.06; 


Obstl  =  [20,20]; 
Obst2  =  [45  ,5]; 
Obst3  =  [28  ,40]; 
Obst4  =  [80,58]; 
Obst5  =  [105  ,92]; 
Obst6  =  [90,120]; 


%======================= 

%  Plot  the  obstacles 
%======================= 

x vertices  =[ Obstl  (1,1),  Obstl  (1,1),  Obstl  (1  , 1  )  +  OBS_SIZE  ,  Obstl  ( 1  , 1 )  + OBS_SIZE ] ; 
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y  vert  ices  =[Obstl  (1,2),  Obstl  (1  ,2 )  + OBS.SIZE  ,  Obstl  (1  ,2)  + OBS.SIZE  ,  Obstl  (1  ,2)]; 
patch(xvertices  ,yvertices  ,  ’  r  ’  ) ; 

% - 


%  Find  the  center  of  obstl 

xl=round((  Obstl  (1  ,1)+ Obstl  (1  , 1  )  +  OBS_SIZE  )/2 ) ; 
yl=round  ((  Obstl  (1  ,2)  +  OBS_SIZE+Obstl  (1  , 2 ) ) / 2 ) ; 

% - 

x  vert  ices  =[Obst2  (1  ,1)  ,  Obst2(l  ,  1)  .  Obst2(l  ,  1  )  +  OBS_SIZE  ,  Obst2  ( 1  ,  1 )  + OBS.SIZE  ] ; 
yvertices  =[Obst2(l  ,2),Obst2(l  ,2)  +  OBS_SIZE  ,  Obst2  ( 1  ,2)  +  OBS_SIZE  ,  Obst2  ( 1  ,2)]; 
patch(xvertices  , yvertices  ,  ’  r  ’  ) ; 

% - 

%  Find  the  center  of  obst2 

x2=round  ( (  Obst2  (1  ,l)+Obst2(l  ,  1  )  +  OBS_SIZE  )/2 ) ; 
y  2= round  ((0bst2(l  ,2)  +  OBS_SIZE+Obst2  (1  ,2))/2); 

% - 

x  vert  ices  =[Obst3  (1  ,1)  ,  Obst3  (1  , 1)  .  Obst3  (1  , 1 )  + OBS.SIZE  ,  Obst3  ( 1  , 1 )  + OBS.SIZE  ] ; 
yvertices  =[Obst3(l  ,2),Obst3(l  ,2  )  +  OBS_SIZE  ,  Obst3  ( 1  ,2)  +  OBS_SIZE  ,  Obst3  ( 1  ,2)]; 
patchfxvertices  , yvertices  ,  ’  r  ’  ) ; 

% - 

%  Find  the  center  of  obst3 

x3=round  ((  Obst3  (1  ,l)+Obst3(l  ,  1  )  +  OBS_SIZE  )/ 2 ) ; 
y3=round  ((  Obst3  (1  ,2)  +  OBS_SIZE+Obst3  (1  ,2))/2); 

% - 

x  vert  ices  =[Obst4(l  ,  1)  ,  Obst4(l  ,  1)  .  Obst4(l  ,  1 )  + OBS.SIZE  ,  Obst4  ( 1  . 11  +  OBS.SIZE  ] ; 

yvertices  =[Obst4(l  ,2)  ,  Obst4(l  ,2 )  +  OBS.SIZE  ,  Obst4  ( 1  ,2)  + OBS.SIZE  ,  Obst4  (1  ,2)]; 

patchfxvertices  , yvertices  ,  ’  r  ’  ) ; 

% - 

%  Find  the  center  of  obst4 

x4=round((Obst4(l  ,l)+Obst4(l  ,  1 )  + OBS.SIZE  )/ 2 ) ; 
y4= round  ((  0bst4(l  ,2)  +  OBS_SIZE+Obst4  ( 1  ,2))/2); 

% - 

x  vert  ices  =[Obst5  (1  , 1)  ,  Obst5  (1  , 1)  ,  Obst5  (1  , 1 )  + OBS.SIZE  ,  Obst5  ( 1  . 1)  +  OBS.SIZE  ] ; 

yvertices  =[  ObstS  ( 1  ,2),Obst5(l  ,2)  +  OBS_SIZE  ,  Obst5  ( 1  ,2)  +  OBS_SIZE  ,  Obst5  ( 1  ,2)]; 

patchfxvertices  , yvertices  ,  ’  r  ’  ) ; 

% - 

%  Find  the  center  of  obst5 

x5=round  ((  Obst5  (1  ,l)+Obst5(l  ,  1  l  +  OBS.SIZE  )/2 ) ; 
y5=round  ((  Obst5  (1  ,2)  +  OBS_SIZE+Obst5  (1  ,2))/2); 

% - 

x  vert  ices  =[Obst6(l  ,1)  ,  Obst6(l  ,1)  ,  Obst6(l  ,  1 )  + OBS.SIZE  ,  Obst6  ( 1  , 1 )  +  OBS.SIZE  ] ; 
yvertices  =[Obst6(l  ,2),Obst6(l  ,2)  +  OBS_SIZE  ,  Obst6  ( 1  ,2)  +  OBS_SIZE  ,  Obst6  ( 1  ,2)]; 
patch(xvertices  , yvertices  ,  ’  r  ’  ) ; 

% - 

%  Find  the  center  of  obst6 

x6= round  ((  0bst6(l  ,l)+Obst6(l  ,  1  )  +  OBS_SIZE  )/2 ) ; 
y6=round  ( (  0bst6  ( 1  ,2 )  +  OBS_SIZE+Obst6  (1  ,2))/2); 

% - 

set(gca,’DataAspectRatio’  ,[1  ,1  ,1]); 
axis  ( [  1  ,140,1  ,140]); 
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t  i  1 1  e  (’ Occupancy  Grid’); 
xlabel  (  ’x’ ); ylabel( ’y’  ); 
drawnow ; 
figure  ( 1 ) ; 


CREATE  GROUND  TRUTH  OCCUPANCY  GRID  FOR  KNOWN  WORLD 


% 


Occupied  =  zeros  (LabDim  ,  LabDim  ) ; 
for  i  =  l:LabDim 

for  j=l: LabDim 

if  ( ( i >=Obstl  (1,1)  &  i<=Obstl  (1  , 1 )  + OBS_SIZE)  &  ( j  >=Obst  1  (1,2) 
&  j  <=Obstl  ( 1  ,2)  +  OBS_SIZE ) ) 

Occupied  ( i  ,  j  )  =  1 ; 

end  ; 

if  ( ( i  >=Obst2  (1,1)  &  i<=Obst2(l  ,  1 )  + OBS_SIZE)  &  ( j  >=Obst2  (1,2) 
&  j<=Obst2(l  ,2)  + OBS_SIZE ) ) 

Occupied  ( i  ,  j  )  =  1 ; 

end  ; 

if  ( ( i >=Obst3  (1,1)  &  i<=Obst3(l  ,  1  )  +  OBS_SIZE)  &  ( j  >=Obst3  (1,2) 
&  j<=Obst3(l  ,2)  + OBS_SIZE ) ) 

Occupied  ( i  ,  j  )  =  1 ; 

end  ; 

if  ( ( i >=Obst4  (1,1)  &  i<=Obst4(l  ,  1  )  +  OBS_SIZE)  &  ( j  >=Obst4  (1,2) 
&  j<=Obst4(l  ,2)  + OBS_SIZE ) ) 

Occupied  ( i  ,  j  )  =  1 ; 

end  ; 

if  ( ( i >=Obst5  (1,1)  &  i<=Obst5(l  ,  1  )  +  OBS_SIZE)  &  ( j  >=Obst5  (1,2) 
&  j<=Obst5(l  ,2)  + OBS_SIZE ) ) 

Occupied  ( i  ,  j  )  =  1 ; 

end  ; 

if  ( ( i >=Obst6  (1,1)  &  i<=Obst6(l  ,  1  )  +  OBS_SIZE)  &  ( j  >=Obst6  ( 1  ,2) 
&  j<=Obst6(l  ,2)  + OBS_SIZE ) ) 

Occupied  ( i  ,  j  )  =  1 ; 

end  ; 

end  ; 

end  ; 


CREATE  A  Quadrotor 


xvertices  =  [—  Sensor_Range  +  ci  ,  Sensor_Range  +  ci  ,  Sensor_Range  +  ci 
—  Sensor  _Range  +  ci ]; 

yvertices  =  [— Sensor_Range  +  ri  Sensor_Range  +  ri  ,  Sensor_Range  +  ri 

Sensor_Range  + r i  ]; 

sensor  =  patch(xvertices  ,  yvertices  ,[.9  ,.9  ,.9]); 

xvertices  =  [— Quadrotor+ci  ,  Quadrotor  +  ci  ,  Quadrotor+ci Quadrotor  +  ci  ] ; 
yvertices  =  [— Quadrotor  +  ri  Quadrotor  +  ri  ,  Quadrotor  +  ri  ,  Quadrotor+ri  ] ; 
robot  =  patch  (  xvertices  , yvertices  ,  ’  ms  ’  )  ; 
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SIMULATION 


for  SimLen=0:5000  %  For  a  simulation  length  of  5000  samples  or  Goal  reach 

%  OBSERVATION  MODEL  % 

wwwwmmmmmmmmmmmmmmmmmmmmmmmmmmmwtfmmmwwc 

%  Generate  a  series  of  readings  for  the  robot  based  on  the  locations  of 
%  obstacles .  The  location  of  the  center  of  the  robot  ,  and  therefore  the 
%  center  of  the  sensor  ,  is  given  by  RobotX  and  RobotY . 

RobotX  =  round  ((max(  xvertices  )  +  min(  xvertices  ))/2) ; 

RobotY  =  round  ((max(  y  vertices  )  +  min(  yvertices  ))/2) ; 

%  TODO  change  the  sensor  model 
%  A  zero  means  nothing  detected 

Observation  =  zeros  (2*  Sensor_Range  ,2*  Sensor_Range  ) ; 
for  i  =  1 :2* Sensor_Range 

for  j =1 :2* Sensor.Range 

SensorX  =  i +RobotX— Sensor_Range  ; 

SensorY=j +RobotY— Sensor.Range  ; 

if  (SensorX<=0)  |  (SensorY<=0)  |  ( SensorX>LabDim)  |  ... 

( SensorY >LabDim) 
continue  ; 

end  ; 

if  (Occupied(  SensorX  ,  SensorY  )==  1 ) 
if  (  rand(l ,  1)  <=  TrueHit ) 

Observation  ( i  ,  j  )  =  1; 
end  ; 

end  ; 

if  (Occupied(  SensorX  ,  SensorY  )  =  =  0) 
if(rand(l,l)  <=  FalseAlarm) 

Observation  ( i  ,  j  )  =  1; 
end  ; 

end  ; 


UPDATE  PROBABILITIES  USING  BAYES  RULE 

For  each  cell  in  the  grid  ,  update  the  probability  of  it  be 
it  be  occupied  using  Bayes  rule  given  the  observation 


% 


%  If  we  observe  a  hit  ===> 
if  Observation  ( i  ,  j  )  ==  1 

Cell.Occ  =  TrueHit  *  p  (  SensorX  ,  SensorY  ) ; 

%  p ( SensorX  ,  SensorY )  is  the  probability  that  a  cell  is  occupied 

Cell.Unocc  =  FalseAlarm  *  (1  —  p  (  SensorX  ,  SensorY  )) ; 

%  1— p (  SensorX  ,  SensorY  )  is  the  probability  that 
%  a  cell  is  unoccupied 
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%  Normalize 

p  ( SensorX  ,  SensorY )  =  Cell.Occ  /  (Cell.Occ  +  Cell.Unocc); 

%  If  do  not  observe  a  hit  => 
else 

Cell.Occ  =  (1  — TrueHit)  *  p  (  SensorX  ,  SensorY  ) ; 

%  p ( SensorX  ,  SensorY )  is  the  probability  that  a  cell  is  occupied 

%  Recall  that  (1  — TrueHit)  is  the  MissedDetection  likelihood 

Cell.Unocc  =  (1  —  False  Alarm  )  *  (1  — p(  SensorX  ,  SensorY  )) ; 

%  1— p ( SensorX  , SensorY )  is  the  prob  that  a  cell  is  unoccupied 

%  (1  —  False  Alarm )  is  the  TrueMiss  likelihood 


%  Normalize 

p  ( SensorX  ,  SensorY )  =  Cell_Occ  /  (Cell_Occ  +  Cell_Unocc); 


end  ; 

%  Update  intensity  to  reflect  probability  that  a  cell  is  occupied. 

%  Darker  colors  mean  higher  probabilities 

set  (  GridHandle  (  SensorX  ,  SensorY  ),’  color  ’  , . . . 

[  1  —  p  (  SensorX  ,  SensorY),l  —  p(  SensorX  ,  SensorY),l  —  p(  SensorX  ,  SensorY  )] ) ; 

end  ; 

end  ; 


ROBOT  MOVEMENT  MODEL 


%=============================================== 

%  Retrieve  the  coordinates  that  define  the  robot 
%=============================================== 


xvertices  =  get(  robot  ,  ’  XData  ’  ) ; 
yvertices  =  get(  robot  ,  ’  YData  *  ) ; 
sensorxvertices  =  get  (  sensor  XData ’) ; 
sensory  vertices  =  get  (  sensor  YData ’) ; 

%============================ 

%  If  within  world  boundaries 
%============================ 

if  (( min  (  x  vertices  +  vel*cos(theta))>l)  &  (max(  x  vertices  + 
vel*cos  ( theta))<LabDim—  Sensor _Range  )) 
xvertices  =  xvertices  +  vel*cos  ( theta  ) ; 
sensorxvertices  =  sensorxvertices  +  vel*cos(theta); 
else  theta  =  pi  / 2  ; 
end  ; 

if  ((  min  (  y  vertices  +  vel*sin(theta))>l)  &  (max(  y  vertice  s  + 
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vel*  sin  ( the ta))<LabDim—  Sensor _Range  )) 
yvertices  =  yvertices  +  vel  *  sin  (  theta  ) ; 
sensoryvertices  =  sensoryvertices  +  vel*sin  (theta); 
else  theta  =  pi  / 2  ; 
end  ; 

%======================================================= 

%  Stop  robot  if  it  is  within  Dealta  tolerance  box  of  goal 
%====================================================== 

if  ((abs  ( RobotX  -  cf)  <=  Del)  &&  (abs  (RobotY  -  rf)  <=  Del)) 
break  ; 

end  ; 

set(robot  ,  ’  XData  ’  ,xvertices  ,  ’  YData  ’  ,  yvertices  ); 

set(sensor  ,  ’  XData  ’  ,sensorxvertices  ,  ’  YData  ’  ,  sensoryvertices  ); 

drawnow  ; 

end  ; 
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APPENDIX  C: 

MATLAB  Code  (Potential  Field) 


The  following  script1  was  created  to  capture  data  from  the  XBee  wireless  module  and  inserted 
into  an  array  data  structure. 

%  Created  by  Mejdi  Ben  Ardhaoui 
%  Modified  by  Professor  Timothy  Chung 


%close  all  ; 

%  Create  Serial  Port 

s  =  s  eri  al  ( ’COM4’  ,  ’BaudRate’,  57600,  ’DataBits’,  8,  ’Parity’,  ’none’, 
’StopBits’,  1,  ’  FlowControl  ’  ,  ’none’); 
disp  (’ Opening  Connection  to  Quadrotor  ’ ) 

%  Open  port  to  start  data  transfer 
fopen ( s  ) ; 

%  Create  an  array  (  Buffer) 
a  =  []; 

%  Total  number  of  beams  used 
N=546; 

%  loop  through  the  beams  and  store  data 
for  i  =  1 : 1 :546%  (N~  =  0) 
a  =  [a  ;  fgetl(s)]; 
end  ; 

%  Convert  string  array  to  number  array 
ranges  =  str2num(a); 

fclose ( s  ) ; 
delete ( s  ) ; 
clear  s 


'All  source  code  is  available  online  at  http :  //f acuity  .nps  .  edu/thchung  under  Resources ,  Software. 
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The  following  script  is  used  to  compute  the  angle  between  a  given  beam  and  an  obstacle  detected 
by  this  beam. 


function  angle_rep  =  compute_angles(posn) 

%  Resolution:  Number  of  beams  divided  by  180  degrees 
t=0. 00576; 

%  Initialize  theta  in  global  coordinates 
theta=posn(3)  —  pi/2; 

%  Compute  angles  between  every  obstacles  and  the  center  of  the  Robot 
for  i  =  1:546 

angle_rep(i)=theta+(i—  l)*t  ; 

end 

return  ; 


Every  time  we  get  a  range  return  different  from  zero,  we  update  the  map  by  changing  the  value 
at  this  position  from  zero  to  one.  One  will  be  interpreted  as  an  obstacle  in  the  other  scripts. 


function  map=updateMap  (posn  ,  ranges  ,  my  lab  ,  map) 

t=0. 00576; 

theta=posn(3)  —  pi/2; 

ranges=round(10*  ranges ); 

for  i  =  1 : 1 : 5 4 6 

X(  i )= round ( posn ( 1 )  +  ranges ( i )* cos (( i * t)— theta  )) ; 

Y(  i )=round (posn (2)  +  ranges ( i )* sin (( i * t)— theta  )) ; 

if  ranges  ( i  )~=0 

%  Test  Borders  limits 

if  (X(  i ) <=0)  ||  (Y(  i )<=0)  ||  (X(i»mylab)  ||  (Y(i»mylab) 
break  ; 

end;  %Don’t  do  anything  outside  the  world  boundaries 
map(X,  Y)=  1 ; 

end 

end 

return  ; 
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This  script  will  change  coordinates  from  Local  to  Global  coordinates. 
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The  following  script  will  draw  and  visualize  the  Robot  and  the  sensor. 


function  [  obs_ptsX  ,  obs_ptsY  ]  =  rob  ( posn  ,  ranges  ) 
rad  =5 ; 
elf  ; 

hold  off ; 
mylab  =  140; 

map=  zeros  (mylab  ,  mylab  ) ; 
axis([0  mylab  0  mylab]); 
axis  xy 
hold  on; 

%draw  a  little  circle  for  the  robot 
angs  =  0:  pi  / 1 0 : 2 *  pi  ; 
x  =  posn(l)  +  rad* cos ( angs ) ; 
y  =  posn(2)  +  rad* sin ( angs  ) ; 
fill  (x  ,  y  ,  ’  b  ’  ) ; 

%posn  (3)=deg2rad(posn  (3)); 

%Draw  line  in  direction  of  heading 
%  line  from  center  of  robot  to  heading  direction 

h.line  =  line  ([  posn  ( 1 )  ,  posn(l)  +  (  rad  )*  cos  ( posn  ( 3  ) )  ]  ,  [posn(2),... 

posn(2)  +  ( rad )* sin ( posn ( 3  ) )  ] ) ; 
set(h_line,  ’Color’,  ’black’,’  Line  Width  ’  ,2) 
ranges=round(10*  ranges ); 
ranges(  (ranges==0)  )  =  max(ranges); 

t=0. 00576; 
theta=posn(3)  —  pi  / 2  ; 
for  i  =  1:1:546 

X( i )= round ( posn ( 1 )  +  ranges ( i )* cos ( theta  +  i * t  )) ; 

Y( i )=round ( posn (2)  +  ranges ( i )* sin ( theta  +  i * t  )) ; 

%plot  (X, Y,  ’g.  ’); 
if  ranges  ( i  )~=0 

%  Test  Borders  limits 
if  X(  i)<=0 
X( i )=0; 

end 

if  Y(  i)<=0 
Y( i )=0; 

end 

if  X(  i)>mylab 
X(  i )  =  m y  1  a b  ; 

end 

if  Y(i)>mylab 
Y(i)  =  mylab; 

end 

%  break ; 
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%  end;  %Don’t  do  anything  outside  the  world  boundaries 

plot  (X, Y,  ’g.  ’  ); 

end 

h_line  =  line  ( [  posn  ( 1 )  ,  posn(l)  +  ranges  ( i  )*  cos  ( theta  +  i  *  t  )]  ,  ... 

[posn(2),  posn(2)  +  ranges ( i )* sin ( theta  +  i * t  )]) ; 
set(h_line,  ’Color’,  ’red’) 

%Draw  line  in  direction  of  heading 
%  line  from  center  of  robot  to  heading  direction 

h.line  =  line  ([  posn  ( 1 )  ,  posn(l)  +  (  rad  )*  cos  (  posn  ( 3 ) )  ]  ,  [posn(2),  ... 

posn(2)  +  ( rad )* sin ( posn ( 3  ) )  ] ) ; 
set(h_line,  ’Color’,  ’black’,’  Line  Width  ,2) 

%TODO  draw  an  arrow  showing  the  resultant  force 
end 

%  Determine  coordinate  locations  for  where  scan  points  hit  obstacles 

%  (less  than  90%  of  max  range) 

obs.ptsX  =  X(  ranges<  0.90* max(  ranges  )  ); 

obs.ptsY  =  Y(  ranges<  0.90* max(  ranges  )  ); 
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In  order  to  determine  the  new  heading,  we  first  need  to  compute  the  repulsive  and  attractive 
forces.  The  resultant  of  these  forces  represent  the  new  heading  for  the  robot  at  this  position. 


compute _force  .m 

function  [SumFrepX  ,  SumFrepY  ,  SumFattX  ,  SumFattY]=  compute  .force  (Xgoal  ,  Ygoal  .  .  . 

,  posn  ,  ranges  ,X,Y,  angle.rep) 

%  ranges(i)  :  distance  from  the  robot  position  to  the  obstacle(i) 

%  dO  :  the  influence  distance  of  the  force 

%  n  :  adjustable  constant  for  the  repulsive  forces. 

%  Frep(i)  :  Repulsive  force  from  obstacle(i) 

%  posn(l)  :  X  postion  of  the  robot 

%  posn(2)  :  Y  postion  of  the  robot 

%  X(  i )  :  X  postion  of  an  obstacle(i) 

%  Y(  i )  :  Y  postion  of  an  obstacle(i) 

%  theta(i)  :  angle  from  robot  position  to  obstacle(i) 

%  Xgoal  :  Goal  X  coordinate 

%  Ygoal  :  Goal  Y  coordinate 

%  k  :  Adjustable  constant  for  the  attraction  force 

%  angle.att  :  angle  between  Goal  and  current  position  of  the  robot 

Initialize  variables 

k  =  3; 
n  =  0.05 ; 
d0  =  1 ; 

%compute  angle  to  goal 

deltaX=Xgoal— posn  ( 1 ) ; 
deltaY = Ygoal— posn  (2) ; 

Dist2Goal  =  sqrt  ((  deltaX  y  2  +  (  deltaY  )A2); 
angle.att  =  atan2  (  deltaY  ,  deltaX); 

Frep  =  [0 ; 0 ] ; 

Fatt  =  [0 ;0] ; 
for  i  =  1 : 546 

if  ((  ranges  ( i  )~  =  0)  &&  (  ranges  ( i)<=d0  ) ) 

%Repulsive  Forces 

FrepX (i)  =  n*cos(angle_rep(i  )); 

FrepY (i)=n*sin(angle_rep(i  )); 

%Attractive  Forces 


FattX(i)=  k*cos  (  angle.att  ) ; 
FattY(i)=  k*  sin  (  angle.att  ) ; 

end 

end 

SumFrepX=— sum  ( FrepX  ) ; 
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SumFrepY=— sum  ( Frep Y  ) 
SumFattX=sum(  FattX  ) ; 
SumFattY=sum(  FattY  ) ; 
end 
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This  is  the  main  code  for  the  project.  This  script  will  call  all  the  scripts  above  in  order  to: 

-  draw  the  map  and  plot  obstacles. 

-  drive  the  robot  from  a  given  start  point  to  the  goal 


clc;  close  all;  clear; 


%  Define  robot  trajectory  data  structure 
posn.hist  =  []; 
obs_pts_hist  =  []; 

%  INITIALIZE  MAP 
hold  off  ; 
mylab  =  140; 

map=  zeros  ( my  lab  ,  my  lab  ) ; 
axis([0  mylab  0  mylab  ] ) ; 
axis  xy 
hold  on ; 

Xgoal  =  input  (’ Input  Goal  X  position:  ’); 

Ygoal  =  input(’Input  Goal  Y  position:  ’); 

%  LOOP  UNTIL  EXITED 
while  ( true  ) 

%  ASK  FOR  USER  INPUT 

%  Either  robot  position 

posn.x  =  input(’Input  robot  X  position:  ’); 

posn.y  =  input(’Input  robot  Y  position:  ’); 

posn_th  =  input(’Input  robot  THETA  heading  in  Radians:  ’); 

posn  =  [posn_x  ,posn_y  ,posn_th]; 

%  Update  history  of  robot  position 
posn_hist  =  [posn_hist;  posn  ] ; 

%  GET  LASER  DATA 

%  Returns  variable:  ranges  ([546x1]  vector  of  range  data) 
GetData 

%  Compute  X  position  and  Y  position  for  obstacles 
[X,  Y]  =  ComputePosn  ( posn  ,  ranges  ,  mylab  )  ; 

%  POPULATE  AND  UPDATE  MAP 

%  Updates  variable  ’map’  with  1/0  occupancy  locations 
map=updateMap  ( posn  ,  ranges  ,  mylab  ,  map ) ; 

%  PLOT  MAP,  ROBOT 

[obs_ptsX,  obs_ptsY]  =  rob  (  posn  ,  ranges  ) ; 
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obs_pts_hist  =  [  obs  _pts  _hist  ;  obs_ptsX  ’  ,  obs_ptsY’]; 


%  Compute  Angles  to  Obstacles 

angle _rep  =  c ompute .angles  ( posn  ) ; 

%  Compute  Forces 

[  SumFrepX  ,  SumFrepY  ,  SumFattX  ,  SumFattY]  =  compute  .force  ( Xgoal  ,  Ygoal  ,  .  .  . 

posn , ranges  ,X,Y, angle.rep  ); 

Fx=SumFrepX+SumFattX  ; 

Fy=SumFrepY+SumFattY  ; 

%Navigation 

posn(l)  =  posn  ( 1  )  +  Fx  *.  005  ; 
posn(2)=  posn  (2)  +  Fy  *  .005  ; 

posn(3)  =  atan2  (Ygoal— posn  (2)  ,  Xgoal— posn  ( 1 )) ; 
figure  (1) 

%  CHECK  IF  EXIT 
%  Check  if  complete 

decision  =  input  (’ Continue  with  next  robot  location?  Y/N:  ’s’); 

if  (  strcmp  (  decision  ,  ’N’  )) 

break  ; 

end 

end  %  end  while(true) 

%  Output  robot  trajectory 

posn.hist 

hold  on; 

%  Plot  all  obstacle  hits 

plot  (  ob s _p ts .hi s t ( :  ,  1 )  ,  obs.pts.hist (:  ,2) ,  ’go’); 

%  Plot  robot  trajectory  on  figure 
plot  (  posn.hist  (:,  1 )  ,  posn.hist  (:  ,2)  ,  ’ b .  —  ’  )  ; 
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